From efabdfd43f7d1c8b551620e703b8bed6acdb8624 Mon Sep 17 00:00:00 2001 From: klaxalk Date: Tue, 9 Apr 2024 22:03:38 +0000 Subject: [PATCH] deploy: 5748756758d6e087134360670ecb85ef232e78b2 --- .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 | 662 ++ index-sort-l.html | 662 ++ index.html | 662 ++ .../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 | 888 ++ .../impl/publisher_handler.hpp.func.html | 888 ++ .../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 | 228 + .../impl/service_client_handler.hpp.func.html | 228 + ...vice_client_handler.hpp.gcov.frameset.html | 19 + .../impl/service_client_handler.hpp.gcov.html | 403 + ...vice_client_handler.hpp.gcov.overview.html | 100 + .../impl/service_client_handler.hpp.gcov.png | Bin 0 -> 969 bytes .../subscribe_handler.hpp.func-sort-c.html | 4256 ++++++++ .../impl/subscribe_handler.hpp.func.html | 4256 ++++++++ .../subscribe_handler.hpp.gcov.frameset.html | 19 + .../impl/subscribe_handler.hpp.gcov.html | 413 + .../subscribe_handler.hpp.gcov.overview.html | 103 + .../impl/subscribe_handler.hpp.gcov.png | Bin 0 -> 1372 bytes .../mrs_lib/impl/timer.hpp.func-sort-c.html | 92 + .../include/mrs_lib/impl/timer.hpp.func.html | 92 + .../mrs_lib/impl/timer.hpp.gcov.frameset.html | 19 + .../include/mrs_lib/impl/timer.hpp.gcov.html | 179 + .../mrs_lib/impl/timer.hpp.gcov.overview.html | 44 + .../include/mrs_lib/impl/timer.hpp.gcov.png | Bin 0 -> 527 bytes .../impl/transformer.hpp.func-sort-c.html | 272 + .../mrs_lib/impl/transformer.hpp.func.html | 272 + .../impl/transformer.hpp.gcov.frameset.html | 19 + .../mrs_lib/impl/transformer.hpp.gcov.html | 280 + .../impl/transformer.hpp.gcov.overview.html | 69 + .../mrs_lib/impl/transformer.hpp.gcov.png | Bin 0 -> 841 bytes .../mrs_lib/impl/ukf.hpp.func-sort-c.html | 128 + .../include/mrs_lib/impl/ukf.hpp.func.html | 128 + .../mrs_lib/impl/ukf.hpp.gcov.frameset.html | 19 + .../include/mrs_lib/impl/ukf.hpp.gcov.html | 364 + .../mrs_lib/impl/ukf.hpp.gcov.overview.html | 90 + mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.png | Bin 0 -> 1198 bytes .../vector_converter.hpp.func-sort-c.html | 152 + .../impl/vector_converter.hpp.func.html | 152 + .../vector_converter.hpp.gcov.frameset.html | 19 + .../impl/vector_converter.hpp.gcov.html | 173 + .../vector_converter.hpp.gcov.overview.html | 43 + .../impl/vector_converter.hpp.gcov.png | Bin 0 -> 456 bytes .../include/mrs_lib/index-detail-sort-f.html | 444 + .../include/mrs_lib/index-detail-sort-l.html | 444 + mrs_lib/include/mrs_lib/index-detail.html | 444 + mrs_lib/include/mrs_lib/index-sort-f.html | 292 + mrs_lib/include/mrs_lib/index-sort-l.html | 292 + mrs_lib/include/mrs_lib/index.html | 292 + .../include/mrs_lib/lkf.h.func-sort-c.html | 268 + mrs_lib/include/mrs_lib/lkf.h.func.html | 268 + .../include/mrs_lib/lkf.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/lkf.h.gcov.html | 460 + .../include/mrs_lib/lkf.h.gcov.overview.html | 114 + mrs_lib/include/mrs_lib/lkf.h.gcov.png | Bin 0 -> 1750 bytes .../mrs_lib/msg_extractor.h.func-sort-c.html | 256 + .../include/mrs_lib/msg_extractor.h.func.html | 256 + .../msg_extractor.h.gcov.frameset.html | 19 + .../include/mrs_lib/msg_extractor.h.gcov.html | 775 ++ .../msg_extractor.h.gcov.overview.html | 193 + .../include/mrs_lib/msg_extractor.h.gcov.png | Bin 0 -> 1287 bytes .../include/mrs_lib/mutex.h.func-sort-c.html | 400 + mrs_lib/include/mrs_lib/mutex.h.func.html | 400 + .../mrs_lib/mutex.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/mutex.h.gcov.html | 260 + .../mrs_lib/mutex.h.gcov.overview.html | 64 + mrs_lib/include/mrs_lib/mutex.h.gcov.png | Bin 0 -> 739 bytes .../mrs_lib/param_loader.h.func-sort-c.html | 392 + .../include/mrs_lib/param_loader.h.func.html | 392 + .../mrs_lib/param_loader.h.gcov.frameset.html | 19 + .../include/mrs_lib/param_loader.h.gcov.html | 1452 +++ .../mrs_lib/param_loader.h.gcov.overview.html | 362 + .../include/mrs_lib/param_loader.h.gcov.png | Bin 0 -> 4406 bytes .../publisher_handler.h.func-sort-c.html | 556 ++ .../mrs_lib/publisher_handler.h.func.html | 556 ++ .../publisher_handler.h.gcov.frameset.html | 19 + .../mrs_lib/publisher_handler.h.gcov.html | 256 + .../publisher_handler.h.gcov.overview.html | 63 + .../mrs_lib/publisher_handler.h.gcov.png | Bin 0 -> 572 bytes ...uadratic_throttle_model.h.func-sort-c.html | 88 + .../quadratic_throttle_model.h.func.html | 88 + ...dratic_throttle_model.h.gcov.frameset.html | 19 + .../quadratic_throttle_model.h.gcov.html | 117 + ...dratic_throttle_model.h.gcov.overview.html | 29 + .../quadratic_throttle_model.h.gcov.png | Bin 0 -> 246 bytes .../mrs_lib/repredictor.h.func-sort-c.html | 256 + .../include/mrs_lib/repredictor.h.func.html | 256 + .../mrs_lib/repredictor.h.gcov.frameset.html | 19 + .../include/mrs_lib/repredictor.h.gcov.html | 637 ++ .../mrs_lib/repredictor.h.gcov.overview.html | 159 + .../include/mrs_lib/repredictor.h.gcov.png | Bin 0 -> 2387 bytes .../safety_zone/index-detail-sort-f.html | 112 + .../safety_zone/index-detail-sort-l.html | 112 + .../mrs_lib/safety_zone/index-detail.html | 112 + .../mrs_lib/safety_zone/index-sort-f.html | 112 + .../mrs_lib/safety_zone/index-sort-l.html | 112 + .../include/mrs_lib/safety_zone/index.html | 112 + .../safety_zone/polygon.h.func-sort-c.html | 92 + .../mrs_lib/safety_zone/polygon.h.func.html | 92 + .../safety_zone/polygon.h.gcov.frameset.html | 19 + .../mrs_lib/safety_zone/polygon.h.gcov.html | 136 + .../safety_zone/polygon.h.gcov.overview.html | 33 + .../mrs_lib/safety_zone/polygon.h.gcov.png | Bin 0 -> 337 bytes .../safety_zone.h.func-sort-c.html | 84 + .../safety_zone/safety_zone.h.func.html | 84 + .../safety_zone.h.gcov.frameset.html | 19 + .../safety_zone/safety_zone.h.gcov.html | 121 + .../safety_zone.h.gcov.overview.html | 30 + .../safety_zone/safety_zone.h.gcov.png | Bin 0 -> 280 bytes .../mrs_lib/scope_timer.h.func-sort-c.html | 92 + .../include/mrs_lib/scope_timer.h.func.html | 92 + .../mrs_lib/scope_timer.h.gcov.frameset.html | 19 + .../include/mrs_lib/scope_timer.h.gcov.html | 248 + .../mrs_lib/scope_timer.h.gcov.overview.html | 61 + .../include/mrs_lib/scope_timer.h.gcov.png | Bin 0 -> 777 bytes .../service_client_handler.h.func-sort-c.html | 152 + .../service_client_handler.h.func.html | 152 + ...ervice_client_handler.h.gcov.frameset.html | 19 + .../service_client_handler.h.gcov.html | 327 + ...ervice_client_handler.h.gcov.overview.html | 81 + .../mrs_lib/service_client_handler.h.gcov.png | Bin 0 -> 689 bytes .../subscribe_handler.h.func-sort-c.html | 3044 ++++++ .../mrs_lib/subscribe_handler.h.func.html | 3044 ++++++ .../subscribe_handler.h.gcov.frameset.html | 19 + .../mrs_lib/subscribe_handler.h.gcov.html | 548 + .../subscribe_handler.h.gcov.overview.html | 136 + .../mrs_lib/subscribe_handler.h.gcov.png | Bin 0 -> 1783 bytes .../timeout_manager.h.func-sort-c.html | 80 + .../mrs_lib/timeout_manager.h.func.html | 80 + .../timeout_manager.h.gcov.frameset.html | 19 + .../mrs_lib/timeout_manager.h.gcov.html | 169 + .../timeout_manager.h.gcov.overview.html | 42 + .../mrs_lib/timeout_manager.h.gcov.png | Bin 0 -> 435 bytes .../include/mrs_lib/timer.h.func-sort-c.html | 108 + mrs_lib/include/mrs_lib/timer.h.func.html | 108 + .../mrs_lib/timer.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/timer.h.gcov.html | 328 + .../mrs_lib/timer.h.gcov.overview.html | 81 + mrs_lib/include/mrs_lib/timer.h.gcov.png | Bin 0 -> 896 bytes .../mrs_lib/transformer.h.func-sort-c.html | 152 + .../include/mrs_lib/transformer.h.func.html | 152 + .../mrs_lib/transformer.h.gcov.frameset.html | 19 + .../include/mrs_lib/transformer.h.gcov.html | 764 ++ .../mrs_lib/transformer.h.gcov.overview.html | 190 + .../include/mrs_lib/transformer.h.gcov.png | Bin 0 -> 2328 bytes .../include/mrs_lib/ukf.h.func-sort-c.html | 88 + mrs_lib/include/mrs_lib/ukf.h.func.html | 88 + .../include/mrs_lib/ukf.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/ukf.h.gcov.html | 285 + .../include/mrs_lib/ukf.h.gcov.overview.html | 71 + mrs_lib/include/mrs_lib/ukf.h.gcov.png | Bin 0 -> 1009 bytes .../include/mrs_lib/utils.h.func-sort-c.html | 100 + mrs_lib/include/mrs_lib/utils.h.func.html | 100 + .../mrs_lib/utils.h.gcov.frameset.html | 19 + mrs_lib/include/mrs_lib/utils.h.gcov.html | 229 + .../mrs_lib/utils.h.gcov.overview.html | 57 + mrs_lib/include/mrs_lib/utils.h.gcov.png | Bin 0 -> 677 bytes .../vector_converter.h.func-sort-c.html | 224 + .../mrs_lib/vector_converter.h.func.html | 224 + .../vector_converter.h.gcov.frameset.html | 19 + .../mrs_lib/vector_converter.h.gcov.html | 128 + .../vector_converter.h.gcov.overview.html | 31 + .../mrs_lib/vector_converter.h.gcov.png | Bin 0 -> 366 bytes .../mrs_lib/visual_object.h.func-sort-c.html | 84 + .../include/mrs_lib/visual_object.h.func.html | 84 + .../visual_object.h.gcov.frameset.html | 19 + .../include/mrs_lib/visual_object.h.gcov.html | 186 + .../visual_object.h.gcov.overview.html | 46 + .../include/mrs_lib/visual_object.h.gcov.png | Bin 0 -> 493 bytes .../attitude_converter.cpp.func-sort-c.html | 244 + .../attitude_converter.cpp.func.html | 244 + .../attitude_converter.cpp.gcov.frameset.html | 19 + .../attitude_converter.cpp.gcov.html | 561 ++ .../attitude_converter.cpp.gcov.overview.html | 140 + .../attitude_converter.cpp.gcov.png | Bin 0 -> 1533 bytes .../index-detail-sort-f.html | 110 + .../index-detail-sort-l.html | 110 + .../src/attitude_converter/index-detail.html | 110 + .../src/attitude_converter/index-sort-f.html | 102 + .../src/attitude_converter/index-sort-l.html | 102 + mrs_lib/src/attitude_converter/index.html | 102 + .../batch_visualizer.cpp.func-sort-c.html | 172 + .../batch_visualizer.cpp.func.html | 172 + .../batch_visualizer.cpp.gcov.frameset.html | 19 + .../batch_visualizer.cpp.gcov.html | 440 + .../batch_visualizer.cpp.gcov.overview.html | 109 + .../batch_visualizer.cpp.gcov.png | Bin 0 -> 1265 bytes .../batch_visualizer/index-detail-sort-f.html | 128 + .../batch_visualizer/index-detail-sort-l.html | 128 + .../src/batch_visualizer/index-detail.html | 128 + .../src/batch_visualizer/index-sort-f.html | 112 + .../src/batch_visualizer/index-sort-l.html | 112 + mrs_lib/src/batch_visualizer/index.html | 112 + .../visual_object.cpp.func-sort-c.html | 168 + .../visual_object.cpp.func.html | 168 + .../visual_object.cpp.gcov.frameset.html | 19 + .../visual_object.cpp.gcov.html | 435 + .../visual_object.cpp.gcov.overview.html | 108 + .../visual_object.cpp.gcov.png | Bin 0 -> 1229 bytes .../geometry/conversions.cpp.func-sort-c.html | 120 + .../src/geometry/conversions.cpp.func.html | 120 + .../conversions.cpp.gcov.frameset.html | 19 + .../src/geometry/conversions.cpp.gcov.html | 178 + .../conversions.cpp.gcov.overview.html | 44 + mrs_lib/src/geometry/conversions.cpp.gcov.png | Bin 0 -> 446 bytes mrs_lib/src/geometry/index-detail-sort-f.html | 138 + mrs_lib/src/geometry/index-detail-sort-l.html | 138 + mrs_lib/src/geometry/index-detail.html | 138 + mrs_lib/src/geometry/index-sort-f.html | 122 + mrs_lib/src/geometry/index-sort-l.html | 122 + mrs_lib/src/geometry/index.html | 122 + .../src/geometry/misc.cpp.func-sort-c.html | 144 + mrs_lib/src/geometry/misc.cpp.func.html | 144 + .../src/geometry/misc.cpp.gcov.frameset.html | 19 + mrs_lib/src/geometry/misc.cpp.gcov.html | 269 + .../src/geometry/misc.cpp.gcov.overview.html | 67 + mrs_lib/src/geometry/misc.cpp.gcov.png | Bin 0 -> 747 bytes .../src/geometry/shapes.cpp.func-sort-c.html | 352 + mrs_lib/src/geometry/shapes.cpp.func.html | 352 + .../geometry/shapes.cpp.gcov.frameset.html | 19 + mrs_lib/src/geometry/shapes.cpp.gcov.html | 730 ++ .../geometry/shapes.cpp.gcov.overview.html | 182 + mrs_lib/src/geometry/shapes.cpp.gcov.png | Bin 0 -> 1596 bytes mrs_lib/src/math/index-detail-sort-f.html | 110 + mrs_lib/src/math/index-detail-sort-l.html | 110 + mrs_lib/src/math/index-detail.html | 110 + mrs_lib/src/math/index-sort-f.html | 102 + mrs_lib/src/math/index-sort-l.html | 102 + mrs_lib/src/math/index.html | 102 + mrs_lib/src/math/math.cpp.func-sort-c.html | 84 + mrs_lib/src/math/math.cpp.func.html | 84 + mrs_lib/src/math/math.cpp.gcov.frameset.html | 19 + mrs_lib/src/math/math.cpp.gcov.html | 149 + mrs_lib/src/math/math.cpp.gcov.overview.html | 37 + mrs_lib/src/math/math.cpp.gcov.png | Bin 0 -> 388 bytes .../median_filter/index-detail-sort-f.html | 110 + .../median_filter/index-detail-sort-l.html | 110 + mrs_lib/src/median_filter/index-detail.html | 110 + mrs_lib/src/median_filter/index-sort-f.html | 102 + mrs_lib/src/median_filter/index-sort-l.html | 102 + mrs_lib/src/median_filter/index.html | 102 + .../median_filter.cpp.func-sort-c.html | 148 + .../median_filter/median_filter.cpp.func.html | 148 + .../median_filter.cpp.gcov.frameset.html | 19 + .../median_filter/median_filter.cpp.gcov.html | 286 + .../median_filter.cpp.gcov.overview.html | 71 + .../median_filter/median_filter.cpp.gcov.png | Bin 0 -> 872 bytes .../src/param_loader/index-detail-sort-f.html | 110 + .../src/param_loader/index-detail-sort-l.html | 110 + mrs_lib/src/param_loader/index-detail.html | 110 + mrs_lib/src/param_loader/index-sort-f.html | 102 + mrs_lib/src/param_loader/index-sort-l.html | 102 + mrs_lib/src/param_loader/index.html | 102 + .../param_provider.cpp.func-sort-c.html | 96 + .../param_loader/param_provider.cpp.func.html | 96 + .../param_provider.cpp.gcov.frameset.html | 19 + .../param_loader/param_provider.cpp.gcov.html | 200 + .../param_provider.cpp.gcov.overview.html | 49 + .../param_loader/param_provider.cpp.gcov.png | Bin 0 -> 659 bytes mrs_lib/src/profiler/index-detail-sort-f.html | 110 + mrs_lib/src/profiler/index-detail-sort-l.html | 110 + mrs_lib/src/profiler/index-detail.html | 110 + mrs_lib/src/profiler/index-sort-f.html | 102 + mrs_lib/src/profiler/index-sort-l.html | 102 + mrs_lib/src/profiler/index.html | 102 + .../profiler/profiler.cpp.func-sort-c.html | 120 + mrs_lib/src/profiler/profiler.cpp.func.html | 120 + .../profiler/profiler.cpp.gcov.frameset.html | 19 + mrs_lib/src/profiler/profiler.cpp.gcov.html | 301 + .../profiler/profiler.cpp.gcov.overview.html | 75 + mrs_lib/src/profiler/profiler.cpp.gcov.png | Bin 0 -> 829 bytes .../src/safety_zone/index-detail-sort-f.html | 128 + .../src/safety_zone/index-detail-sort-l.html | 128 + mrs_lib/src/safety_zone/index-detail.html | 128 + mrs_lib/src/safety_zone/index-sort-f.html | 112 + mrs_lib/src/safety_zone/index-sort-l.html | 112 + mrs_lib/src/safety_zone/index.html | 112 + .../line_operations.cpp.func-sort-c.html | 92 + .../safety_zone/line_operations.cpp.func.html | 92 + .../line_operations.cpp.gcov.frameset.html | 19 + .../safety_zone/line_operations.cpp.gcov.html | 143 + .../line_operations.cpp.gcov.overview.html | 35 + .../safety_zone/line_operations.cpp.gcov.png | Bin 0 -> 465 bytes .../polygon/index-detail-sort-f.html | 110 + .../polygon/index-detail-sort-l.html | 110 + .../src/safety_zone/polygon/index-detail.html | 110 + .../src/safety_zone/polygon/index-sort-f.html | 102 + .../src/safety_zone/polygon/index-sort-l.html | 102 + mrs_lib/src/safety_zone/polygon/index.html | 102 + .../polygon/polygon.cpp.func-sort-c.html | 112 + .../safety_zone/polygon/polygon.cpp.func.html | 112 + .../polygon/polygon.cpp.gcov.frameset.html | 19 + .../safety_zone/polygon/polygon.cpp.gcov.html | 290 + .../polygon/polygon.cpp.gcov.overview.html | 72 + .../safety_zone/polygon/polygon.cpp.gcov.png | Bin 0 -> 1057 bytes .../safety_zone.cpp.func-sort-c.html | 104 + .../src/safety_zone/safety_zone.cpp.func.html | 104 + .../safety_zone.cpp.gcov.frameset.html | 19 + .../src/safety_zone/safety_zone.cpp.gcov.html | 160 + .../safety_zone.cpp.gcov.overview.html | 39 + .../src/safety_zone/safety_zone.cpp.gcov.png | Bin 0 -> 380 bytes .../src/scope_timer/index-detail-sort-f.html | 110 + .../src/scope_timer/index-detail-sort-l.html | 110 + mrs_lib/src/scope_timer/index-detail.html | 110 + mrs_lib/src/scope_timer/index-sort-f.html | 102 + mrs_lib/src/scope_timer/index-sort-l.html | 102 + mrs_lib/src/scope_timer/index.html | 102 + .../scope_timer.cpp.func-sort-c.html | 120 + .../src/scope_timer/scope_timer.cpp.func.html | 120 + .../scope_timer.cpp.gcov.frameset.html | 19 + .../src/scope_timer/scope_timer.cpp.gcov.html | 345 + .../scope_timer.cpp.gcov.overview.html | 86 + .../src/scope_timer/scope_timer.cpp.gcov.png | Bin 0 -> 1280 bytes .../timeout_manager/index-detail-sort-f.html | 110 + .../timeout_manager/index-detail-sort-l.html | 110 + mrs_lib/src/timeout_manager/index-detail.html | 110 + mrs_lib/src/timeout_manager/index-sort-f.html | 102 + mrs_lib/src/timeout_manager/index-sort-l.html | 102 + mrs_lib/src/timeout_manager/index.html | 102 + .../timeout_manager.cpp.func-sort-c.html | 124 + .../timeout_manager.cpp.func.html | 124 + .../timeout_manager.cpp.gcov.frameset.html | 19 + .../timeout_manager.cpp.gcov.html | 186 + .../timeout_manager.cpp.gcov.overview.html | 46 + .../timeout_manager.cpp.gcov.png | Bin 0 -> 512 bytes mrs_lib/src/timer/index-detail-sort-f.html | 110 + mrs_lib/src/timer/index-detail-sort-l.html | 110 + mrs_lib/src/timer/index-detail.html | 110 + mrs_lib/src/timer/index-sort-f.html | 102 + mrs_lib/src/timer/index-sort-l.html | 102 + mrs_lib/src/timer/index.html | 102 + mrs_lib/src/timer/timer.cpp.func-sort-c.html | 140 + mrs_lib/src/timer/timer.cpp.func.html | 140 + .../src/timer/timer.cpp.gcov.frameset.html | 19 + mrs_lib/src/timer/timer.cpp.gcov.html | 339 + .../src/timer/timer.cpp.gcov.overview.html | 84 + mrs_lib/src/timer/timer.cpp.gcov.png | Bin 0 -> 1067 bytes .../index-detail-sort-f.html | 110 + .../index-detail-sort-l.html | 110 + .../transform_broadcaster/index-detail.html | 110 + .../transform_broadcaster/index-sort-f.html | 102 + .../transform_broadcaster/index-sort-l.html | 102 + mrs_lib/src/transform_broadcaster/index.html | 102 + ...transform_broadcaster.cpp.func-sort-c.html | 92 + .../transform_broadcaster.cpp.func.html | 92 + ...ansform_broadcaster.cpp.gcov.frameset.html | 19 + .../transform_broadcaster.cpp.gcov.html | 140 + ...ansform_broadcaster.cpp.gcov.overview.html | 34 + .../transform_broadcaster.cpp.gcov.png | Bin 0 -> 425 bytes .../src/transformer/index-detail-sort-f.html | 110 + .../src/transformer/index-detail-sort-l.html | 110 + mrs_lib/src/transformer/index-detail.html | 110 + mrs_lib/src/transformer/index-sort-f.html | 102 + mrs_lib/src/transformer/index-sort-l.html | 102 + mrs_lib/src/transformer/index.html | 102 + .../transformer.cpp.func-sort-c.html | 184 + .../src/transformer/transformer.cpp.func.html | 184 + .../transformer.cpp.gcov.frameset.html | 19 + .../src/transformer/transformer.cpp.gcov.html | 671 ++ .../transformer.cpp.gcov.overview.html | 167 + .../src/transformer/transformer.cpp.gcov.png | Bin 0 -> 1995 bytes mrs_lib/src/utils/index-detail-sort-f.html | 110 + mrs_lib/src/utils/index-detail-sort-l.html | 110 + mrs_lib/src/utils/index-detail.html | 110 + mrs_lib/src/utils/index-sort-f.html | 102 + mrs_lib/src/utils/index-sort-l.html | 102 + mrs_lib/src/utils/index.html | 102 + mrs_lib/src/utils/utils.cpp.func-sort-c.html | 88 + mrs_lib/src/utils/utils.cpp.func.html | 88 + .../src/utils/utils.cpp.gcov.frameset.html | 19 + mrs_lib/src/utils/utils.cpp.gcov.html | 101 + .../src/utils/utils.cpp.gcov.overview.html | 25 + mrs_lib/src/utils/utils.cpp.gcov.png | Bin 0 -> 194 bytes .../src/automatic_start.cpp.func-sort-c.html | 164 + .../src/automatic_start.cpp.func.html | 164 + .../automatic_start.cpp.gcov.frameset.html | 19 + .../src/automatic_start.cpp.gcov.html | 1049 ++ .../automatic_start.cpp.gcov.overview.html | 262 + .../src/automatic_start.cpp.gcov.png | Bin 0 -> 3123 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 | 2214 +++++ .../src/mpc_controller.cpp.gcov.overview.html | 553 ++ .../src/mpc_controller.cpp.gcov.png | Bin 0 -> 6652 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 | 1927 ++++ .../src/se3_controller.cpp.gcov.overview.html | 481 + .../src/se3_controller.cpp.gcov.png | Bin 0 -> 5838 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 -> 934 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 -> 505 bytes .../index-detail-sort-f.html | 128 + .../index-detail-sort-l.html | 128 + .../estimation_manager/index-detail.html | 128 + .../estimation_manager/index-sort-f.html | 112 + .../estimation_manager/index-sort-l.html | 112 + .../estimation_manager/index.html | 112 + .../support.h.func-sort-c.html | 140 + .../estimation_manager/support.h.func.html | 140 + .../support.h.gcov.frameset.html | 19 + .../estimation_manager/support.h.gcov.html | 405 + .../support.h.gcov.overview.html | 101 + .../estimation_manager/support.h.gcov.png | Bin 0 -> 1232 bytes .../mrs_uav_managers/index-detail-sort-f.html | 164 + .../mrs_uav_managers/index-detail-sort-l.html | 164 + .../mrs_uav_managers/index-detail.html | 164 + .../mrs_uav_managers/index-sort-f.html | 132 + .../mrs_uav_managers/index-sort-l.html | 132 + .../include/mrs_uav_managers/index.html | 132 + .../state_estimator.h.func-sort-c.html | 92 + .../state_estimator.h.func.html | 92 + .../state_estimator.h.gcov.frameset.html | 19 + .../state_estimator.h.gcov.html | 169 + .../state_estimator.h.gcov.overview.html | 42 + .../state_estimator.h.gcov.png | Bin 0 -> 435 bytes .../tracker.h.func-sort-c.html | 88 + .../mrs_uav_managers/tracker.h.func.html | 88 + .../tracker.h.gcov.frameset.html | 19 + .../mrs_uav_managers/tracker.h.gcov.html | 296 + .../tracker.h.gcov.overview.html | 73 + .../mrs_uav_managers/tracker.h.gcov.png | Bin 0 -> 751 bytes .../index-detail-sort-f.html | 120 + .../index-detail-sort-l.html | 120 + .../transform_manager/index-detail.html | 120 + .../transform_manager/index-sort-f.html | 112 + .../transform_manager/index-sort-l.html | 112 + .../include/transform_manager/index.html | 112 + .../tf_mapping_origin.h.func-sort-c.html | 100 + .../tf_mapping_origin.h.func.html | 100 + .../tf_mapping_origin.h.gcov.frameset.html | 19 + .../tf_mapping_origin.h.gcov.html | 474 + .../tf_mapping_origin.h.gcov.overview.html | 118 + .../tf_mapping_origin.h.gcov.png | Bin 0 -> 1536 bytes .../tf_source.h.func-sort-c.html | 136 + .../transform_manager/tf_source.h.func.html | 136 + .../tf_source.h.gcov.frameset.html | 19 + .../transform_manager/tf_source.h.gcov.html | 704 ++ .../tf_source.h.gcov.overview.html | 175 + .../transform_manager/tf_source.h.gcov.png | Bin 0 -> 2210 bytes .../constraint_manager.cpp.func-sort-c.html | 112 + .../src/constraint_manager.cpp.func.html | 112 + .../constraint_manager.cpp.gcov.frameset.html | 19 + .../src/constraint_manager.cpp.gcov.html | 716 ++ .../constraint_manager.cpp.gcov.overview.html | 178 + .../src/constraint_manager.cpp.gcov.png | Bin 0 -> 2202 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 -> 2428 bytes .../common/index-detail-sort-f.html | 110 + .../common/index-detail-sort-l.html | 110 + .../control_manager/common/index-detail.html | 110 + .../control_manager/common/index-sort-f.html | 102 + .../control_manager/common/index-sort-l.html | 102 + .../src/control_manager/common/index.html | 102 + .../control_manager.cpp.func-sort-c.html | 524 + .../control_manager.cpp.func.html | 524 + .../control_manager.cpp.gcov.frameset.html | 19 + .../control_manager.cpp.gcov.html | 8839 +++++++++++++++++ .../control_manager.cpp.gcov.overview.html | 2209 ++++ .../control_manager.cpp.gcov.png | Bin 0 -> 25983 bytes .../control_manager/index-detail-sort-f.html | 128 + .../control_manager/index-detail-sort-l.html | 128 + .../src/control_manager/index-detail.html | 128 + .../src/control_manager/index-sort-f.html | 112 + .../src/control_manager/index-sort-l.html | 112 + .../src/control_manager/index.html | 112 + .../output_publisher.cpp.func-sort-c.html | 128 + .../output_publisher.cpp.func.html | 128 + .../output_publisher.cpp.gcov.frameset.html | 19 + .../output_publisher.cpp.gcov.html | 154 + .../output_publisher.cpp.gcov.overview.html | 38 + .../output_publisher.cpp.gcov.png | Bin 0 -> 307 bytes .../estimation_manager.cpp.func-sort-c.html | 176 + .../estimation_manager.cpp.func.html | 176 + .../estimation_manager.cpp.gcov.frameset.html | 19 + .../estimation_manager.cpp.gcov.html | 1330 +++ .../estimation_manager.cpp.gcov.overview.html | 332 + .../estimation_manager.cpp.gcov.png | Bin 0 -> 4322 bytes .../estimator.cpp.func-sort-c.html | 172 + .../estimator.cpp.func.html | 172 + .../estimator.cpp.gcov.frameset.html | 19 + .../estimator.cpp.gcov.html | 290 + .../estimator.cpp.gcov.overview.html | 72 + .../estimation_manager/estimator.cpp.gcov.png | Bin 0 -> 805 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 -> 2126 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 | 1037 ++ .../transform_manager.cpp.gcov.overview.html | 259 + .../transform_manager.cpp.gcov.png | Bin 0 -> 3373 bytes .../src/uav_manager.cpp.func-sort-c.html | 236 + .../src/uav_manager.cpp.func.html | 236 + .../src/uav_manager.cpp.gcov.frameset.html | 19 + .../src/uav_manager.cpp.gcov.html | 2794 ++++++ .../src/uav_manager.cpp.gcov.overview.html | 698 ++ mrs_uav_managers/src/uav_manager.cpp.gcov.png | Bin 0 -> 7432 bytes .../agl/garmin_agl.h.func-sort-c.html | 92 + .../estimators/agl/garmin_agl.h.func.html | 92 + .../agl/garmin_agl.h.gcov.frameset.html | 19 + .../estimators/agl/garmin_agl.h.gcov.html | 159 + .../agl/garmin_agl.h.gcov.overview.html | 39 + .../estimators/agl/garmin_agl.h.gcov.png | Bin 0 -> 380 bytes .../estimators/agl/index-detail-sort-f.html | 110 + .../estimators/agl/index-detail-sort-l.html | 110 + .../estimators/agl/index-detail.html | 110 + .../estimators/agl/index-sort-f.html | 102 + .../estimators/agl/index-sort-l.html | 102 + .../estimators/agl/index.html | 102 + .../altitude/alt_generic.h.func-sort-c.html | 92 + .../altitude/alt_generic.h.func.html | 92 + .../altitude/alt_generic.h.gcov.frameset.html | 19 + .../altitude/alt_generic.h.gcov.html | 245 + .../altitude/alt_generic.h.gcov.overview.html | 61 + .../altitude/alt_generic.h.gcov.png | Bin 0 -> 688 bytes .../altitude_estimator.h.func-sort-c.html | 92 + .../altitude/altitude_estimator.h.func.html | 92 + .../altitude_estimator.h.gcov.frameset.html | 19 + .../altitude/altitude_estimator.h.gcov.html | 130 + .../altitude_estimator.h.gcov.overview.html | 32 + .../altitude/altitude_estimator.h.gcov.png | Bin 0 -> 295 bytes .../altitude/index-detail-sort-f.html | 128 + .../altitude/index-detail-sort-l.html | 128 + .../estimators/altitude/index-detail.html | 128 + .../estimators/altitude/index-sort-f.html | 112 + .../estimators/altitude/index-sort-l.html | 112 + .../estimators/altitude/index.html | 112 + .../estimators/correction.h.func-sort-c.html | 364 + .../estimators/correction.h.func.html | 364 + .../correction.h.gcov.frameset.html | 19 + .../estimators/correction.h.gcov.html | 1944 ++++ .../correction.h.gcov.overview.html | 485 + .../estimators/correction.h.gcov.png | Bin 0 -> 5362 bytes .../heading/hdg_generic.h.func-sort-c.html | 92 + .../heading/hdg_generic.h.func.html | 92 + .../heading/hdg_generic.h.gcov.frameset.html | 19 + .../heading/hdg_generic.h.gcov.html | 243 + .../heading/hdg_generic.h.gcov.overview.html | 60 + .../estimators/heading/hdg_generic.h.gcov.png | Bin 0 -> 663 bytes .../hdg_passthrough.h.func-sort-c.html | 92 + .../heading/hdg_passthrough.h.func.html | 92 + .../hdg_passthrough.h.gcov.frameset.html | 19 + .../heading/hdg_passthrough.h.gcov.html | 191 + .../hdg_passthrough.h.gcov.overview.html | 47 + .../heading/hdg_passthrough.h.gcov.png | Bin 0 -> 506 bytes .../heading_estimator.h.func-sort-c.html | 84 + .../heading/heading_estimator.h.func.html | 84 + .../heading_estimator.h.gcov.frameset.html | 19 + .../heading/heading_estimator.h.gcov.html | 124 + .../heading_estimator.h.gcov.overview.html | 30 + .../heading/heading_estimator.h.gcov.png | Bin 0 -> 293 bytes .../heading/index-detail-sort-f.html | 138 + .../heading/index-detail-sort-l.html | 138 + .../estimators/heading/index-detail.html | 138 + .../estimators/heading/index-sort-f.html | 122 + .../estimators/heading/index-sort-l.html | 122 + .../estimators/heading/index.html | 122 + .../estimators/index-detail-sort-f.html | 128 + .../estimators/index-detail-sort-l.html | 128 + .../estimators/index-detail.html | 128 + .../estimators/index-sort-f.html | 112 + .../estimators/index-sort-l.html | 112 + .../estimators/index.html | 112 + .../lateral/index-detail-sort-f.html | 128 + .../lateral/index-detail-sort-l.html | 128 + .../estimators/lateral/index-detail.html | 128 + .../estimators/lateral/index-sort-f.html | 112 + .../estimators/lateral/index-sort-l.html | 112 + .../estimators/lateral/index.html | 112 + .../lateral/lat_generic.h.func-sort-c.html | 92 + .../lateral/lat_generic.h.func.html | 92 + .../lateral/lat_generic.h.gcov.frameset.html | 19 + .../lateral/lat_generic.h.gcov.html | 250 + .../lateral/lat_generic.h.gcov.overview.html | 62 + .../estimators/lateral/lat_generic.h.gcov.png | Bin 0 -> 708 bytes .../lateral_estimator.h.func-sort-c.html | 92 + .../lateral/lateral_estimator.h.func.html | 92 + .../lateral_estimator.h.gcov.frameset.html | 19 + .../lateral/lateral_estimator.h.gcov.html | 122 + .../lateral_estimator.h.gcov.overview.html | 30 + .../lateral/lateral_estimator.h.gcov.png | Bin 0 -> 284 bytes .../partial_estimator.h.func-sort-c.html | 176 + .../estimators/partial_estimator.h.func.html | 176 + .../partial_estimator.h.gcov.frameset.html | 19 + .../estimators/partial_estimator.h.gcov.html | 264 + .../partial_estimator.h.gcov.overview.html | 65 + .../estimators/partial_estimator.h.gcov.png | Bin 0 -> 820 bytes .../estimators/state/index-detail-sort-f.html | 128 + .../estimators/state/index-detail-sort-l.html | 128 + .../estimators/state/index-detail.html | 128 + .../estimators/state/index-sort-f.html | 112 + .../estimators/state/index-sort-l.html | 112 + .../estimators/state/index.html | 112 + .../state/passthrough.h.func-sort-c.html | 92 + .../estimators/state/passthrough.h.func.html | 92 + .../state/passthrough.h.gcov.frameset.html | 19 + .../estimators/state/passthrough.h.gcov.html | 173 + .../state/passthrough.h.gcov.overview.html | 43 + .../estimators/state/passthrough.h.gcov.png | Bin 0 -> 431 bytes .../state/state_generic.h.func-sort-c.html | 92 + .../state/state_generic.h.func.html | 92 + .../state/state_generic.h.gcov.frameset.html | 19 + .../state/state_generic.h.gcov.html | 183 + .../state/state_generic.h.gcov.overview.html | 45 + .../estimators/state/state_generic.h.gcov.png | Bin 0 -> 438 bytes .../processors/index-detail-sort-f.html | 182 + .../processors/index-detail-sort-l.html | 182 + .../processors/index-detail.html | 182 + .../processors/index-sort-f.html | 142 + .../processors/index-sort-l.html | 142 + .../processors/index.html | 142 + .../proc_excessive_tilt.h.func-sort-c.html | 104 + .../proc_excessive_tilt.h.func.html | 104 + .../proc_excessive_tilt.h.gcov.frameset.html | 19 + .../proc_excessive_tilt.h.gcov.html | 206 + .../proc_excessive_tilt.h.gcov.overview.html | 51 + .../processors/proc_excessive_tilt.h.gcov.png | Bin 0 -> 661 bytes .../proc_median_filter.h.func-sort-c.html | 104 + .../processors/proc_median_filter.h.func.html | 104 + .../proc_median_filter.h.gcov.frameset.html | 19 + .../processors/proc_median_filter.h.gcov.html | 192 + .../proc_median_filter.h.gcov.overview.html | 47 + .../processors/proc_median_filter.h.gcov.png | Bin 0 -> 635 bytes .../proc_saturate.h.func-sort-c.html | 104 + .../processors/proc_saturate.h.func.html | 104 + .../proc_saturate.h.gcov.frameset.html | 19 + .../processors/proc_saturate.h.gcov.html | 202 + .../proc_saturate.h.gcov.overview.html | 50 + .../processors/proc_saturate.h.gcov.png | Bin 0 -> 711 bytes .../proc_tf_to_world.h.func-sort-c.html | 112 + .../processors/proc_tf_to_world.h.func.html | 112 + .../proc_tf_to_world.h.gcov.frameset.html | 19 + .../processors/proc_tf_to_world.h.gcov.html | 241 + .../proc_tf_to_world.h.gcov.overview.html | 60 + .../processors/proc_tf_to_world.h.gcov.png | Bin 0 -> 777 bytes .../processors/processor.h.func-sort-c.html | 112 + .../processors/processor.h.func.html | 112 + .../processors/processor.h.gcov.frameset.html | 19 + .../processors/processor.h.gcov.html | 156 + .../processors/processor.h.gcov.overview.html | 38 + .../processors/processor.h.gcov.png | Bin 0 -> 436 bytes .../agl/garmin_agl.cpp.func-sort-c.html | 120 + .../estimators/agl/garmin_agl.cpp.func.html | 120 + .../agl/garmin_agl.cpp.gcov.frameset.html | 19 + .../estimators/agl/garmin_agl.cpp.gcov.html | 318 + .../agl/garmin_agl.cpp.gcov.overview.html | 79 + .../estimators/agl/garmin_agl.cpp.gcov.png | Bin 0 -> 1043 bytes .../estimators/agl/index-detail-sort-f.html | 110 + .../estimators/agl/index-detail-sort-l.html | 110 + .../src/estimators/agl/index-detail.html | 110 + .../src/estimators/agl/index-sort-f.html | 102 + .../src/estimators/agl/index-sort-l.html | 102 + .../src/estimators/agl/index.html | 102 + .../altitude/alt_generic.cpp.func-sort-c.html | 212 + .../altitude/alt_generic.cpp.func.html | 212 + .../alt_generic.cpp.gcov.frameset.html | 19 + .../altitude/alt_generic.cpp.gcov.html | 839 ++ .../alt_generic.cpp.gcov.overview.html | 209 + .../altitude/alt_generic.cpp.gcov.png | Bin 0 -> 2916 bytes .../altitude/index-detail-sort-f.html | 110 + .../altitude/index-detail-sort-l.html | 110 + .../src/estimators/altitude/index-detail.html | 110 + .../src/estimators/altitude/index-sort-f.html | 102 + .../src/estimators/altitude/index-sort-l.html | 102 + .../src/estimators/altitude/index.html | 102 + .../heading/hdg_generic.cpp.func-sort-c.html | 216 + .../heading/hdg_generic.cpp.func.html | 216 + .../hdg_generic.cpp.gcov.frameset.html | 19 + .../heading/hdg_generic.cpp.gcov.html | 795 ++ .../hdg_generic.cpp.gcov.overview.html | 198 + .../heading/hdg_generic.cpp.gcov.png | Bin 0 -> 2308 bytes .../hdg_passthrough.cpp.func-sort-c.html | 172 + .../heading/hdg_passthrough.cpp.func.html | 172 + .../hdg_passthrough.cpp.gcov.frameset.html | 19 + .../heading/hdg_passthrough.cpp.gcov.html | 391 + .../hdg_passthrough.cpp.gcov.overview.html | 97 + .../heading/hdg_passthrough.cpp.gcov.png | Bin 0 -> 1198 bytes .../heading/index-detail-sort-f.html | 120 + .../heading/index-detail-sort-l.html | 120 + .../src/estimators/heading/index-detail.html | 120 + .../src/estimators/heading/index-sort-f.html | 112 + .../src/estimators/heading/index-sort-l.html | 112 + .../src/estimators/heading/index.html | 112 + .../lateral/index-detail-sort-f.html | 110 + .../lateral/index-detail-sort-l.html | 110 + .../src/estimators/lateral/index-detail.html | 110 + .../src/estimators/lateral/index-sort-f.html | 102 + .../src/estimators/lateral/index-sort-l.html | 102 + .../src/estimators/lateral/index.html | 102 + .../lateral/lat_generic.cpp.func-sort-c.html | 212 + .../lateral/lat_generic.cpp.func.html | 212 + .../lat_generic.cpp.gcov.frameset.html | 19 + .../lateral/lat_generic.cpp.gcov.html | 887 ++ .../lat_generic.cpp.gcov.overview.html | 221 + .../lateral/lat_generic.cpp.gcov.png | Bin 0 -> 3146 bytes .../state/gps_baro.cpp.func-sort-c.html | 96 + .../estimators/state/gps_baro.cpp.func.html | 96 + .../state/gps_baro.cpp.gcov.frameset.html | 19 + .../estimators/state/gps_baro.cpp.gcov.html | 109 + .../state/gps_baro.cpp.gcov.overview.html | 27 + .../estimators/state/gps_baro.cpp.gcov.png | Bin 0 -> 231 bytes .../state/gps_garmin.cpp.func-sort-c.html | 96 + .../estimators/state/gps_garmin.cpp.func.html | 96 + .../state/gps_garmin.cpp.gcov.frameset.html | 19 + .../estimators/state/gps_garmin.cpp.gcov.html | 109 + .../state/gps_garmin.cpp.gcov.overview.html | 27 + .../estimators/state/gps_garmin.cpp.gcov.png | Bin 0 -> 231 bytes .../estimators/state/index-detail-sort-f.html | 200 + .../estimators/state/index-detail-sort-l.html | 200 + .../src/estimators/state/index-detail.html | 200 + .../src/estimators/state/index-sort-f.html | 152 + .../src/estimators/state/index-sort-l.html | 152 + .../src/estimators/state/index.html | 152 + .../state/passthrough.cpp.func-sort-c.html | 116 + .../state/passthrough.cpp.func.html | 116 + .../state/passthrough.cpp.gcov.frameset.html | 19 + .../state/passthrough.cpp.gcov.html | 343 + .../state/passthrough.cpp.gcov.overview.html | 85 + .../estimators/state/passthrough.cpp.gcov.png | Bin 0 -> 1099 bytes .../estimators/state/rtk.cpp.func-sort-c.html | 96 + .../src/estimators/state/rtk.cpp.func.html | 96 + .../state/rtk.cpp.gcov.frameset.html | 19 + .../src/estimators/state/rtk.cpp.gcov.html | 109 + .../state/rtk.cpp.gcov.overview.html | 27 + .../src/estimators/state/rtk.cpp.gcov.png | Bin 0 -> 229 bytes .../state/rtk_garmin.cpp.func-sort-c.html | 96 + .../estimators/state/rtk_garmin.cpp.func.html | 96 + .../state/rtk_garmin.cpp.gcov.frameset.html | 19 + .../estimators/state/rtk_garmin.cpp.gcov.html | 109 + .../state/rtk_garmin.cpp.gcov.overview.html | 27 + .../estimators/state/rtk_garmin.cpp.gcov.png | Bin 0 -> 231 bytes .../state/state_generic.cpp.func-sort-c.html | 128 + .../state/state_generic.cpp.func.html | 128 + .../state_generic.cpp.gcov.frameset.html | 19 + .../state/state_generic.cpp.gcov.html | 633 ++ .../state_generic.cpp.gcov.overview.html | 158 + .../state/state_generic.cpp.gcov.png | Bin 0 -> 1968 bytes .../src/joy_tracker/index-detail-sort-f.html | 110 + .../src/joy_tracker/index-detail-sort-l.html | 110 + .../src/joy_tracker/index-detail.html | 110 + .../src/joy_tracker/index-sort-f.html | 102 + .../src/joy_tracker/index-sort-l.html | 102 + mrs_uav_trackers/src/joy_tracker/index.html | 102 + .../joy_tracker.cpp.func-sort-c.html | 152 + .../src/joy_tracker/joy_tracker.cpp.func.html | 152 + .../joy_tracker.cpp.gcov.frameset.html | 19 + .../src/joy_tracker/joy_tracker.cpp.gcov.html | 588 ++ .../joy_tracker.cpp.gcov.overview.html | 146 + .../src/joy_tracker/joy_tracker.cpp.gcov.png | Bin 0 -> 1740 bytes .../landoff_tracker/index-detail-sort-f.html | 110 + .../landoff_tracker/index-detail-sort-l.html | 110 + .../src/landoff_tracker/index-detail.html | 110 + .../src/landoff_tracker/index-sort-f.html | 102 + .../src/landoff_tracker/index-sort-l.html | 102 + .../src/landoff_tracker/index.html | 102 + .../landoff_tracker.cpp.func-sort-c.html | 204 + .../landoff_tracker.cpp.func.html | 204 + .../landoff_tracker.cpp.gcov.frameset.html | 19 + .../landoff_tracker.cpp.gcov.html | 1636 +++ .../landoff_tracker.cpp.gcov.overview.html | 408 + .../landoff_tracker.cpp.gcov.png | Bin 0 -> 4772 bytes .../src/line_tracker/index-detail-sort-f.html | 110 + .../src/line_tracker/index-detail-sort-l.html | 110 + .../src/line_tracker/index-detail.html | 110 + .../src/line_tracker/index-sort-f.html | 102 + .../src/line_tracker/index-sort-l.html | 102 + mrs_uav_trackers/src/line_tracker/index.html | 102 + .../line_tracker.cpp.func-sort-c.html | 200 + .../line_tracker/line_tracker.cpp.func.html | 200 + .../line_tracker.cpp.gcov.frameset.html | 19 + .../line_tracker/line_tracker.cpp.gcov.html | 1359 +++ .../line_tracker.cpp.gcov.overview.html | 339 + .../line_tracker/line_tracker.cpp.gcov.png | Bin 0 -> 3774 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 | 3931 ++++++++ .../mpc_tracker.cpp.gcov.overview.html | 982 ++ .../src/mpc_tracker/mpc_tracker.cpp.gcov.png | Bin 0 -> 12412 bytes .../speed_tracker/index-detail-sort-f.html | 110 + .../speed_tracker/index-detail-sort-l.html | 110 + .../src/speed_tracker/index-detail.html | 110 + .../src/speed_tracker/index-sort-f.html | 102 + .../src/speed_tracker/index-sort-l.html | 102 + mrs_uav_trackers/src/speed_tracker/index.html | 102 + .../speed_tracker.cpp.func-sort-c.html | 156 + .../speed_tracker/speed_tracker.cpp.func.html | 156 + .../speed_tracker.cpp.gcov.frameset.html | 19 + .../speed_tracker/speed_tracker.cpp.gcov.html | 1178 +++ .../speed_tracker.cpp.gcov.overview.html | 294 + .../speed_tracker/speed_tracker.cpp.gcov.png | Bin 0 -> 3011 bytes .../eth_mav_msgs/common.h.func-sort-c.html | 88 + .../include/eth_mav_msgs/common.h.func.html | 88 + .../eth_mav_msgs/common.h.gcov.frameset.html | 19 + .../include/eth_mav_msgs/common.h.gcov.html | 496 + .../eth_mav_msgs/common.h.gcov.overview.html | 123 + .../include/eth_mav_msgs/common.h.gcov.png | Bin 0 -> 1579 bytes .../eigen_mav_msgs.h.func-sort-c.html | 100 + .../eth_mav_msgs/eigen_mav_msgs.h.func.html | 100 + .../eigen_mav_msgs.h.gcov.frameset.html | 19 + .../eth_mav_msgs/eigen_mav_msgs.h.gcov.html | 463 + .../eigen_mav_msgs.h.gcov.overview.html | 115 + .../eth_mav_msgs/eigen_mav_msgs.h.gcov.png | Bin 0 -> 1499 bytes .../eth_mav_msgs/index-detail-sort-f.html | 128 + .../eth_mav_msgs/index-detail-sort-l.html | 128 + .../include/eth_mav_msgs/index-detail.html | 128 + .../include/eth_mav_msgs/index-sort-f.html | 112 + .../include/eth_mav_msgs/index-sort-l.html | 112 + .../include/eth_mav_msgs/index.html | 112 + .../extremum.h.func-sort-c.html | 96 + .../extremum.h.func.html | 96 + .../extremum.h.gcov.frameset.html | 19 + .../extremum.h.gcov.html | 143 + .../extremum.h.gcov.overview.html | 35 + .../extremum.h.gcov.png | Bin 0 -> 437 bytes .../impl/index-detail-sort-f.html | 128 + .../impl/index-detail-sort-l.html | 128 + .../impl/index-detail.html | 128 + .../impl/index-sort-f.html | 112 + .../impl/index-sort-l.html | 112 + .../eth_trajectory_generation/impl/index.html | 112 + ...ptimization_linear_impl.h.func-sort-c.html | 136 + ...omial_optimization_linear_impl.h.func.html | 136 + ...imization_linear_impl.h.gcov.frameset.html | 19 + ...omial_optimization_linear_impl.h.gcov.html | 708 ++ ...imization_linear_impl.h.gcov.overview.html | 176 + ...nomial_optimization_linear_impl.h.gcov.png | Bin 0 -> 2764 bytes ...mization_nonlinear_impl.h.func-sort-c.html | 156 + ...al_optimization_nonlinear_impl.h.func.html | 156 + ...zation_nonlinear_impl.h.gcov.frameset.html | 19 + ...al_optimization_nonlinear_impl.h.gcov.html | 933 ++ ...zation_nonlinear_impl.h.gcov.overview.html | 233 + ...ial_optimization_nonlinear_impl.h.gcov.png | Bin 0 -> 3493 bytes .../index-detail-sort-f.html | 228 + .../index-detail-sort-l.html | 228 + .../index-detail.html | 228 + .../index-sort-f.html | 172 + .../index-sort-l.html | 172 + .../eth_trajectory_generation/index.html | 172 + .../polynomial.h.func-sort-c.html | 120 + .../polynomial.h.func.html | 120 + .../polynomial.h.gcov.frameset.html | 19 + .../polynomial.h.gcov.html | 353 + .../polynomial.h.gcov.overview.html | 88 + .../polynomial.h.gcov.png | Bin 0 -> 1544 bytes ...ial_optimization_linear.h.func-sort-c.html | 124 + ...polynomial_optimization_linear.h.func.html | 124 + ...l_optimization_linear.h.gcov.frameset.html | 19 + ...polynomial_optimization_linear.h.gcov.html | 401 + ...l_optimization_linear.h.gcov.overview.html | 100 + .../polynomial_optimization_linear.h.gcov.png | Bin 0 -> 1749 bytes ..._optimization_nonlinear.h.func-sort-c.html | 92 + ...ynomial_optimization_nonlinear.h.func.html | 92 + ...ptimization_nonlinear.h.gcov.frameset.html | 19 + ...ynomial_optimization_nonlinear.h.gcov.html | 399 + ...ptimization_nonlinear.h.gcov.overview.html | 99 + ...lynomial_optimization_nonlinear.h.gcov.png | Bin 0 -> 1665 bytes .../segment.h.func-sort-c.html | 104 + .../segment.h.func.html | 104 + .../segment.h.gcov.frameset.html | 19 + .../segment.h.gcov.html | 231 + .../segment.h.gcov.overview.html | 57 + .../segment.h.gcov.png | Bin 0 -> 898 bytes .../timing.h.func-sort-c.html | 120 + .../timing.h.func.html | 120 + .../timing.h.gcov.frameset.html | 19 + .../timing.h.gcov.html | 322 + .../timing.h.gcov.overview.html | 80 + .../timing.h.gcov.png | Bin 0 -> 1027 bytes .../trajectory.h.func-sort-c.html | 128 + .../trajectory.h.func.html | 128 + .../trajectory.h.gcov.frameset.html | 19 + .../trajectory.h.gcov.html | 261 + .../trajectory.h.gcov.overview.html | 65 + .../trajectory.h.gcov.png | Bin 0 -> 973 bytes .../vertex.h.func-sort-c.html | 96 + .../vertex.h.func.html | 96 + .../vertex.h.gcov.frameset.html | 19 + .../vertex.h.gcov.html | 256 + .../vertex.h.gcov.overview.html | 63 + .../vertex.h.gcov.png | Bin 0 -> 1052 bytes .../index-detail-sort-f.html | 202 + .../index-detail-sort-l.html | 202 + .../index-detail.html | 202 + .../index-sort-f.html | 162 + .../index-sort-l.html | 162 + .../src/eth_trajectory_generation/index.html | 162 + .../motion_defines.cpp.func-sort-c.html | 96 + .../motion_defines.cpp.func.html | 96 + .../motion_defines.cpp.gcov.frameset.html | 19 + .../motion_defines.cpp.gcov.html | 174 + .../motion_defines.cpp.gcov.overview.html | 43 + .../motion_defines.cpp.gcov.png | Bin 0 -> 473 bytes .../polynomial.cpp.func-sort-c.html | 124 + .../polynomial.cpp.func.html | 124 + .../polynomial.cpp.gcov.frameset.html | 19 + .../polynomial.cpp.gcov.html | 325 + .../polynomial.cpp.gcov.overview.html | 81 + .../polynomial.cpp.gcov.png | Bin 0 -> 1168 bytes .../rpoly/index-detail-sort-f.html | 110 + .../rpoly/index-detail-sort-l.html | 110 + .../rpoly/index-detail.html | 110 + .../rpoly/index-sort-f.html | 102 + .../rpoly/index-sort-l.html | 102 + .../rpoly/index.html | 102 + .../rpoly/rpoly_ak1.cpp.func-sort-c.html | 128 + .../rpoly/rpoly_ak1.cpp.func.html | 128 + .../rpoly/rpoly_ak1.cpp.gcov.frameset.html | 19 + .../rpoly/rpoly_ak1.cpp.gcov.html | 1027 ++ .../rpoly/rpoly_ak1.cpp.gcov.overview.html | 256 + .../rpoly/rpoly_ak1.cpp.gcov.png | Bin 0 -> 4844 bytes .../segment.cpp.func-sort-c.html | 132 + .../segment.cpp.func.html | 132 + .../segment.cpp.gcov.frameset.html | 19 + .../segment.cpp.gcov.html | 399 + .../segment.cpp.gcov.overview.html | 99 + .../segment.cpp.gcov.png | Bin 0 -> 1496 bytes .../timing.cpp.func-sort-c.html | 200 + .../timing.cpp.func.html | 200 + .../timing.cpp.gcov.frameset.html | 19 + .../timing.cpp.gcov.html | 415 + .../timing.cpp.gcov.overview.html | 103 + .../timing.cpp.gcov.png | Bin 0 -> 1088 bytes .../trajectory.cpp.func-sort-c.html | 172 + .../trajectory.cpp.func.html | 172 + .../trajectory.cpp.gcov.frameset.html | 19 + .../trajectory.cpp.gcov.html | 780 ++ .../trajectory.cpp.gcov.overview.html | 194 + .../trajectory.cpp.gcov.png | Bin 0 -> 2513 bytes .../trajectory_sampling.cpp.func-sort-c.html | 108 + .../trajectory_sampling.cpp.func.html | 108 + ...trajectory_sampling.cpp.gcov.frameset.html | 19 + .../trajectory_sampling.cpp.gcov.html | 271 + ...trajectory_sampling.cpp.gcov.overview.html | 67 + .../trajectory_sampling.cpp.gcov.png | Bin 0 -> 941 bytes .../vertex.cpp.func-sort-c.html | 148 + .../vertex.cpp.func.html | 148 + .../vertex.cpp.gcov.frameset.html | 19 + .../vertex.cpp.gcov.html | 674 ++ .../vertex.cpp.gcov.overview.html | 168 + .../vertex.cpp.gcov.png | Bin 0 -> 2241 bytes .../src/index-detail-sort-f.html | 110 + .../src/index-detail-sort-l.html | 110 + .../src/index-detail.html | 110 + .../src/index-sort-f.html | 102 + .../src/index-sort-l.html | 102 + mrs_uav_trajectory_generation/src/index.html | 102 + ...trajectory_generation.cpp.func-sort-c.html | 168 + .../mrs_trajectory_generation.cpp.func.html | 168 + ...ajectory_generation.cpp.gcov.frameset.html | 19 + .../mrs_trajectory_generation.cpp.gcov.html | 2458 +++++ ...ajectory_generation.cpp.gcov.overview.html | 614 ++ .../mrs_trajectory_generation.cpp.gcov.png | Bin 0 -> 7274 bytes ruby.png | Bin 0 -> 141 bytes snow.png | Bin 0 -> 141 bytes updown.png | Bin 0 -> 117 bytes 1170 files changed, 200061 insertions(+) create mode 100644 .nojekyll create mode 100644 amber.png create mode 100644 badge.svg create mode 100644 emerald.png create mode 100644 gcov.css create mode 100644 glass.png create mode 100644 index-sort-f.html create mode 100644 index-sort-l.html create mode 100644 index.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.func.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/attitude_converter.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.func.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.func.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/geometry/index-detail-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index-detail-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index-detail.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/geometry/index.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.func.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/geometry/misc.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.func.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/gps_conversions.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/index-detail-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/impl/index-detail-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/impl/index-detail.html create mode 100644 mrs_lib/include/mrs_lib/impl/index-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/impl/index-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/impl/index.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.func.html create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.html create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.png create mode 100644 mrs_lib/include/mrs_lib/index-detail-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/index-detail-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/index-detail.html create mode 100644 mrs_lib/include/mrs_lib/index-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/index-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/index.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.func.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/lkf.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.func.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/msg_extractor.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/mutex.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.func.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/mutex.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.func.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/param_loader.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.func.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/publisher_handler.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.func.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/repredictor.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-detail.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-sort-f.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index-sort-l.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/index.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.func.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.func.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/scope_timer.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.func.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/service_client_handler.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.func.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.func.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/timeout_manager.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/timer.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.func.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/timer.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/transformer.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.func.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/transformer.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/ukf.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.func.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/ukf.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/utils.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.func.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/utils.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.func.html create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/vector_converter.h.gcov.png create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.func-sort-c.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.func.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.gcov.frameset.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.gcov.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.gcov.overview.html create mode 100644 mrs_lib/include/mrs_lib/visual_object.h.gcov.png create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.func-sort-c.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.func.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.overview.html create mode 100644 mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.png create mode 100644 mrs_lib/src/attitude_converter/index-detail-sort-f.html create mode 100644 mrs_lib/src/attitude_converter/index-detail-sort-l.html create mode 100644 mrs_lib/src/attitude_converter/index-detail.html create mode 100644 mrs_lib/src/attitude_converter/index-sort-f.html create mode 100644 mrs_lib/src/attitude_converter/index-sort-l.html create mode 100644 mrs_lib/src/attitude_converter/index.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func-sort-c.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.overview.html create mode 100644 mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.png create mode 100644 mrs_lib/src/batch_visualizer/index-detail-sort-f.html create mode 100644 mrs_lib/src/batch_visualizer/index-detail-sort-l.html create mode 100644 mrs_lib/src/batch_visualizer/index-detail.html create mode 100644 mrs_lib/src/batch_visualizer/index-sort-f.html create mode 100644 mrs_lib/src/batch_visualizer/index-sort-l.html create mode 100644 mrs_lib/src/batch_visualizer/index.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.func-sort-c.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.func.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.overview.html create mode 100644 mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.png create mode 100644 mrs_lib/src/geometry/conversions.cpp.func-sort-c.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.func.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.gcov.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.gcov.overview.html create mode 100644 mrs_lib/src/geometry/conversions.cpp.gcov.png create mode 100644 mrs_lib/src/geometry/index-detail-sort-f.html create mode 100644 mrs_lib/src/geometry/index-detail-sort-l.html create mode 100644 mrs_lib/src/geometry/index-detail.html create mode 100644 mrs_lib/src/geometry/index-sort-f.html create mode 100644 mrs_lib/src/geometry/index-sort-l.html create mode 100644 mrs_lib/src/geometry/index.html create mode 100644 mrs_lib/src/geometry/misc.cpp.func-sort-c.html create mode 100644 mrs_lib/src/geometry/misc.cpp.func.html create mode 100644 mrs_lib/src/geometry/misc.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/geometry/misc.cpp.gcov.html create mode 100644 mrs_lib/src/geometry/misc.cpp.gcov.overview.html create mode 100644 mrs_lib/src/geometry/misc.cpp.gcov.png create mode 100644 mrs_lib/src/geometry/shapes.cpp.func-sort-c.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.func.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.gcov.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.gcov.overview.html create mode 100644 mrs_lib/src/geometry/shapes.cpp.gcov.png create mode 100644 mrs_lib/src/math/index-detail-sort-f.html create mode 100644 mrs_lib/src/math/index-detail-sort-l.html create mode 100644 mrs_lib/src/math/index-detail.html create mode 100644 mrs_lib/src/math/index-sort-f.html create mode 100644 mrs_lib/src/math/index-sort-l.html create mode 100644 mrs_lib/src/math/index.html create mode 100644 mrs_lib/src/math/math.cpp.func-sort-c.html create mode 100644 mrs_lib/src/math/math.cpp.func.html create mode 100644 mrs_lib/src/math/math.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/math/math.cpp.gcov.html create mode 100644 mrs_lib/src/math/math.cpp.gcov.overview.html create mode 100644 mrs_lib/src/math/math.cpp.gcov.png create mode 100644 mrs_lib/src/median_filter/index-detail-sort-f.html create mode 100644 mrs_lib/src/median_filter/index-detail-sort-l.html create mode 100644 mrs_lib/src/median_filter/index-detail.html create mode 100644 mrs_lib/src/median_filter/index-sort-f.html create mode 100644 mrs_lib/src/median_filter/index-sort-l.html create mode 100644 mrs_lib/src/median_filter/index.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.func-sort-c.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.func.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.gcov.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.gcov.overview.html create mode 100644 mrs_lib/src/median_filter/median_filter.cpp.gcov.png create mode 100644 mrs_lib/src/param_loader/index-detail-sort-f.html create mode 100644 mrs_lib/src/param_loader/index-detail-sort-l.html create mode 100644 mrs_lib/src/param_loader/index-detail.html create mode 100644 mrs_lib/src/param_loader/index-sort-f.html create mode 100644 mrs_lib/src/param_loader/index-sort-l.html create mode 100644 mrs_lib/src/param_loader/index.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.func-sort-c.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.func.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.gcov.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.gcov.overview.html create mode 100644 mrs_lib/src/param_loader/param_provider.cpp.gcov.png create mode 100644 mrs_lib/src/profiler/index-detail-sort-f.html create mode 100644 mrs_lib/src/profiler/index-detail-sort-l.html create mode 100644 mrs_lib/src/profiler/index-detail.html create mode 100644 mrs_lib/src/profiler/index-sort-f.html create mode 100644 mrs_lib/src/profiler/index-sort-l.html create mode 100644 mrs_lib/src/profiler/index.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.func-sort-c.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.func.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.gcov.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.gcov.overview.html create mode 100644 mrs_lib/src/profiler/profiler.cpp.gcov.png create mode 100644 mrs_lib/src/safety_zone/index-detail-sort-f.html create mode 100644 mrs_lib/src/safety_zone/index-detail-sort-l.html create mode 100644 mrs_lib/src/safety_zone/index-detail.html create mode 100644 mrs_lib/src/safety_zone/index-sort-f.html create mode 100644 mrs_lib/src/safety_zone/index-sort-l.html create mode 100644 mrs_lib/src/safety_zone/index.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.func-sort-c.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.func.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.gcov.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.gcov.overview.html create mode 100644 mrs_lib/src/safety_zone/line_operations.cpp.gcov.png create mode 100644 mrs_lib/src/safety_zone/polygon/index-detail-sort-f.html create mode 100644 mrs_lib/src/safety_zone/polygon/index-detail-sort-l.html create mode 100644 mrs_lib/src/safety_zone/polygon/index-detail.html create mode 100644 mrs_lib/src/safety_zone/polygon/index-sort-f.html create mode 100644 mrs_lib/src/safety_zone/polygon/index-sort-l.html create mode 100644 mrs_lib/src/safety_zone/polygon/index.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.func-sort-c.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.func.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.overview.html create mode 100644 mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.png create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.func-sort-c.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.func.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.gcov.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.gcov.overview.html create mode 100644 mrs_lib/src/safety_zone/safety_zone.cpp.gcov.png create mode 100644 mrs_lib/src/scope_timer/index-detail-sort-f.html create mode 100644 mrs_lib/src/scope_timer/index-detail-sort-l.html create mode 100644 mrs_lib/src/scope_timer/index-detail.html create mode 100644 mrs_lib/src/scope_timer/index-sort-f.html create mode 100644 mrs_lib/src/scope_timer/index-sort-l.html create mode 100644 mrs_lib/src/scope_timer/index.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.func-sort-c.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.func.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.gcov.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.gcov.overview.html create mode 100644 mrs_lib/src/scope_timer/scope_timer.cpp.gcov.png create mode 100644 mrs_lib/src/timeout_manager/index-detail-sort-f.html create mode 100644 mrs_lib/src/timeout_manager/index-detail-sort-l.html create mode 100644 mrs_lib/src/timeout_manager/index-detail.html create mode 100644 mrs_lib/src/timeout_manager/index-sort-f.html create mode 100644 mrs_lib/src/timeout_manager/index-sort-l.html create mode 100644 mrs_lib/src/timeout_manager/index.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.func-sort-c.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.func.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html create mode 100644 mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.png create mode 100644 mrs_lib/src/timer/index-detail-sort-f.html create mode 100644 mrs_lib/src/timer/index-detail-sort-l.html create mode 100644 mrs_lib/src/timer/index-detail.html create mode 100644 mrs_lib/src/timer/index-sort-f.html create mode 100644 mrs_lib/src/timer/index-sort-l.html create mode 100644 mrs_lib/src/timer/index.html create mode 100644 mrs_lib/src/timer/timer.cpp.func-sort-c.html create mode 100644 mrs_lib/src/timer/timer.cpp.func.html create mode 100644 mrs_lib/src/timer/timer.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/timer/timer.cpp.gcov.html create mode 100644 mrs_lib/src/timer/timer.cpp.gcov.overview.html create mode 100644 mrs_lib/src/timer/timer.cpp.gcov.png create mode 100644 mrs_lib/src/transform_broadcaster/index-detail-sort-f.html create mode 100644 mrs_lib/src/transform_broadcaster/index-detail-sort-l.html create mode 100644 mrs_lib/src/transform_broadcaster/index-detail.html create mode 100644 mrs_lib/src/transform_broadcaster/index-sort-f.html create mode 100644 mrs_lib/src/transform_broadcaster/index-sort-l.html create mode 100644 mrs_lib/src/transform_broadcaster/index.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func-sort-c.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.overview.html create mode 100644 mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.png create mode 100644 mrs_lib/src/transformer/index-detail-sort-f.html create mode 100644 mrs_lib/src/transformer/index-detail-sort-l.html create mode 100644 mrs_lib/src/transformer/index-detail.html create mode 100644 mrs_lib/src/transformer/index-sort-f.html create mode 100644 mrs_lib/src/transformer/index-sort-l.html create mode 100644 mrs_lib/src/transformer/index.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.func-sort-c.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.func.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.gcov.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.gcov.overview.html create mode 100644 mrs_lib/src/transformer/transformer.cpp.gcov.png create mode 100644 mrs_lib/src/utils/index-detail-sort-f.html create mode 100644 mrs_lib/src/utils/index-detail-sort-l.html create mode 100644 mrs_lib/src/utils/index-detail.html create mode 100644 mrs_lib/src/utils/index-sort-f.html create mode 100644 mrs_lib/src/utils/index-sort-l.html create mode 100644 mrs_lib/src/utils/index.html create mode 100644 mrs_lib/src/utils/utils.cpp.func-sort-c.html create mode 100644 mrs_lib/src/utils/utils.cpp.func.html create mode 100644 mrs_lib/src/utils/utils.cpp.gcov.frameset.html create mode 100644 mrs_lib/src/utils/utils.cpp.gcov.html create mode 100644 mrs_lib/src/utils/utils.cpp.gcov.overview.html create mode 100644 mrs_lib/src/utils/utils.cpp.gcov.png create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.func-sort-c.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.func.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.gcov.frameset.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.gcov.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.gcov.overview.html create mode 100644 mrs_uav_autostart/src/automatic_start.cpp.gcov.png create mode 100644 mrs_uav_autostart/src/index-detail-sort-f.html create mode 100644 mrs_uav_autostart/src/index-detail-sort-l.html create mode 100644 mrs_uav_autostart/src/index-detail.html create mode 100644 mrs_uav_autostart/src/index-sort-f.html create mode 100644 mrs_uav_autostart/src/index-sort-l.html create mode 100644 mrs_uav_autostart/src/index.html create mode 100644 mrs_uav_controllers/include/common.h.func-sort-c.html create mode 100644 mrs_uav_controllers/include/common.h.func.html create mode 100644 mrs_uav_controllers/include/common.h.gcov.frameset.html create mode 100644 mrs_uav_controllers/include/common.h.gcov.html create mode 100644 mrs_uav_controllers/include/common.h.gcov.overview.html create mode 100644 mrs_uav_controllers/include/common.h.gcov.png create mode 100644 mrs_uav_controllers/include/index-detail-sort-f.html create mode 100644 mrs_uav_controllers/include/index-detail-sort-l.html create mode 100644 mrs_uav_controllers/include/index-detail.html create mode 100644 mrs_uav_controllers/include/index-sort-f.html create mode 100644 mrs_uav_controllers/include/index-sort-l.html create mode 100644 mrs_uav_controllers/include/index.html create mode 100644 mrs_uav_controllers/include/pid.hpp.func-sort-c.html create mode 100644 mrs_uav_controllers/include/pid.hpp.func.html create mode 100644 mrs_uav_controllers/include/pid.hpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/include/pid.hpp.gcov.html create mode 100644 mrs_uav_controllers/include/pid.hpp.gcov.overview.html create mode 100644 mrs_uav_controllers/include/pid.hpp.gcov.png create mode 100644 mrs_uav_controllers/src/common.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/common.cpp.func.html create mode 100644 mrs_uav_controllers/src/common.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/common.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/common.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/common.cpp.gcov.png create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.func.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/failsafe_controller.cpp.gcov.png create mode 100644 mrs_uav_controllers/src/index-detail-sort-f.html create mode 100644 mrs_uav_controllers/src/index-detail-sort-l.html create mode 100644 mrs_uav_controllers/src/index-detail.html create mode 100644 mrs_uav_controllers/src/index-sort-f.html create mode 100644 mrs_uav_controllers/src/index-sort-l.html create mode 100644 mrs_uav_controllers/src/index.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.func.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.png create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.func.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/mpc_controller.cpp.gcov.png create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.func-sort-c.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.func.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.gcov.frameset.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.gcov.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.gcov.overview.html create mode 100644 mrs_uav_controllers/src/se3_controller.cpp.gcov.png create mode 100644 mrs_uav_managers/include/control_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/control_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/control_manager/index-detail.html create mode 100644 mrs_uav_managers/include/control_manager/index-sort-f.html create mode 100644 mrs_uav_managers/include/control_manager/index-sort-l.html create mode 100644 mrs_uav_managers/include/control_manager/index.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.func.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.gcov.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/control_manager/output_publisher.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/common.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/control_manager/index.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-detail.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-sort-f.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index-sort-l.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/index.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.png create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.func.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.png create mode 100644 mrs_uav_managers/include/transform_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/include/transform_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/include/transform_manager/index-detail.html create mode 100644 mrs_uav_managers/include/transform_manager/index-sort-f.html create mode 100644 mrs_uav_managers/include/transform_manager/index-sort-l.html create mode 100644 mrs_uav_managers/include/transform_manager/index.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.png create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.func-sort-c.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.func.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.gcov.frameset.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.gcov.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.gcov.overview.html create mode 100644 mrs_uav_managers/include/transform_manager/tf_source.h.gcov.png create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/constraint_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.func.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.gcov.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/control_manager/common/common.cpp.gcov.png create mode 100644 mrs_uav_managers/src/control_manager/common/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/control_manager/common/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/control_manager/common/index-detail.html create mode 100644 mrs_uav_managers/src/control_manager/common/index-sort-f.html create mode 100644 mrs_uav_managers/src/control_manager/common/index-sort-l.html create mode 100644 mrs_uav_managers/src/control_manager/common/index.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/control_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/control_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/control_manager/index-detail.html create mode 100644 mrs_uav_managers/src/control_manager/index-sort-f.html create mode 100644 mrs_uav_managers/src/control_manager/index-sort-l.html create mode 100644 mrs_uav_managers/src/control_manager/index.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.func.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.func.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.func.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-detail.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-sort-f.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index-sort-l.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/index.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.png create mode 100644 mrs_uav_managers/src/estimation_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/estimation_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/estimation_manager/index-detail.html create mode 100644 mrs_uav_managers/src/estimation_manager/index-sort-f.html create mode 100644 mrs_uav_managers/src/estimation_manager/index-sort-l.html create mode 100644 mrs_uav_managers/src/estimation_manager/index.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/gain_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/index-detail.html create mode 100644 mrs_uav_managers/src/index-sort-f.html create mode 100644 mrs_uav_managers/src/index-sort-l.html create mode 100644 mrs_uav_managers/src/index.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.func.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.gcov.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/null_tracker.cpp.gcov.png create mode 100644 mrs_uav_managers/src/transform_manager/index-detail-sort-f.html create mode 100644 mrs_uav_managers/src/transform_manager/index-detail-sort-l.html create mode 100644 mrs_uav_managers/src/transform_manager/index-detail.html create mode 100644 mrs_uav_managers/src/transform_manager/index-sort-f.html create mode 100644 mrs_uav_managers/src/transform_manager/index-sort-l.html create mode 100644 mrs_uav_managers/src/transform_manager/index.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.png create mode 100644 mrs_uav_managers/src/uav_manager.cpp.func-sort-c.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.func.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.gcov.frameset.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.gcov.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.gcov.overview.html create mode 100644 mrs_uav_managers/src/uav_manager.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-f.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-l.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.png create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func-sort-c.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.overview.html create mode 100644 mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/agl/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/altitude/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/heading/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-detail-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-detail-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-detail.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-sort-f.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index-sort-l.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/index.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.png create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func-sort-c.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.frameset.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html create mode 100644 mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/joy_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/joy_tracker/index.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/index.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/line_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/line_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/line_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/line_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/line_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/line_tracker/index.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/index.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/index.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.png create mode 100644 mrs_uav_trackers/src/speed_tracker/index-detail-sort-f.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index-detail-sort-l.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index-detail.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index-sort-f.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index-sort-l.html create mode 100644 mrs_uav_trackers/src/speed_tracker/index.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func-sort-c.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.frameset.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.overview.html create mode 100644 mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail-sort-f.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail-sort-l.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/index-sort-f.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/index-sort-l.html create mode 100644 mrs_uav_trajectory_generation/include/eth_mav_msgs/index.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail-sort-f.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail-sort-l.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-sort-f.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-sort-l.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail-sort-f.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail-sort-l.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-sort-f.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-sort-l.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/index.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.func.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail-sort-f.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail-sort-l.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-sort-f.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-sort-l.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/index.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail-sort-f.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail-sort-l.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-sort-f.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-sort-l.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.png create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.func-sort-c.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.func.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.frameset.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.overview.html create mode 100644 mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.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 coverage61.4%61.4% \ 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..832ab45631 --- /dev/null +++ b/index-sort-f.html @@ -0,0 +1,662 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:152112365864.3 %
Date:2024-04-09 22:03:33Functions:2680436461.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Directory Sort by nameLine Coverage Sort by line coverageFunctions Sort by function coverage
mrs_lib/include/mrs_lib/safety_zone +
0.0%
+ 		0.0 %0 / 80.0 %0 / 4
mrs_lib/src/geometry +
15.1%15.1%
+ 		15.1 %67 / 44516.0 %15 / 94
mrs_uav_state_estimators/src/estimators/heading +
18.6%18.6%
+ 		18.6 %91 / 49021.1 %12 / 57
mrs_uav_trajectory_generation/src/eth_trajectory_generation +
43.6%43.6%
+ 		43.6 %452 / 103629.5 %31 / 105
mrs_uav_trackers/src/speed_tracker +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
mrs_uav_trackers/src/joy_tracker +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
mrs_lib/include/mrs_lib/geometry +
71.0%71.0%
+ 		71.0 %66 / 9342.5 %51 / 120
mrs_lib/src/batch_visualizer +
44.1%44.1%
+ 		44.1 %192 / 43544.4 %20 / 45
mrs_lib/src/safety_zone/polygon +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
mrs_lib/src/scope_timer +
14.1%14.1%
+ 		14.1 %20 / 14250.0 %5 / 10
mrs_uav_trackers/src/midair_activation_tracker +
72.8%72.8%
+ 		72.8 %67 / 9250.0 %9 / 18
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors +
72.3%72.3%
+ 		72.3 %133 / 18450.0 %17 / 34
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading +
58.3%58.3%
+ 		58.3 %7 / 1257.1 %4 / 7
mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl +
49.1%49.1%
+ 		49.1 %316 / 64357.6 %19 / 33
mrs_lib/include/mrs_lib/impl +
82.3%82.3%
+ 		82.3 %367 / 44658.4 %806 / 1380
mrs_uav_managers/include/mrs_uav_managers +
100.0%
+ 		100.0 %12 / 1260.0 %6 / 10
mrs_uav_state_estimators/src/estimators/agl +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
mrs_uav_trajectory_generation/include/eth_trajectory_generation +
62.9%62.9%
+ 		62.9 %129 / 20561.7 %37 / 60
mrs_lib/include/mrs_lib +
79.3%79.3%
+ 		79.3 %781 / 98561.9 %833 / 1345
mrs_uav_trackers/src/line_tracker +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
mrs_lib/src/transform_broadcaster +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
mrs_uav_state_estimators/src/estimators/altitude +
53.9%53.9%
+ 		53.9 %213 / 39566.7 %22 / 33
mrs_uav_trackers/src/landoff_tracker +
73.1%73.1%
+ 		73.1 %433 / 59267.7 %21 / 31
mrs_uav_managers/src/control_manager/common +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 31
mrs_uav_managers/include/transform_manager +
48.0%48.0%
+ 		48.0 %191 / 39868.4 %13 / 19
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
53.0%53.0%
+ 		53.0 %403 / 76169.5 %66 / 95
mrs_uav_managers/include/mrs_uav_managers/control_manager +
55.2%55.2%
+ 		55.2 %53 / 9670.4 %19 / 27
mrs_uav_managers/src/control_manager +
64.1%64.1%
+ 		64.1 %2368 / 369472.4 %89 / 123
mrs_uav_state_estimators/src/estimators/state +
70.7%70.7%
+ 		70.7 %311 / 44075.7 %28 / 37
mrs_uav_state_estimators/src/estimators/lateral +
55.1%55.1%
+ 		55.1 %241 / 43775.8 %25 / 33
mrs_lib/src/transformer +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
mrs_uav_managers/src +
66.2%66.2%
+ 		66.2 %1145 / 172978.4 %58 / 74
mrs_uav_trackers/src/mpc_tracker +
80.0%80.0%
+ 		80.0 %1320 / 165080.0 %40 / 50
mrs_lib/src/timeout_manager +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/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_managers/src/estimation_manager +
70.0%70.0%
+ 		70.0 %433 / 61985.1 %40 / 47
mrs_uav_controllers/src +
81.0%81.0%
+ 		81.0 %1760 / 217287.9 %58 / 66
mrs_lib/src/safety_zone +
63.3%63.3%
+ 		63.3 %31 / 4988.9 %8 / 9
mrs_lib/src/profiler +
37.6%37.6%
+ 		37.6 %38 / 10190.0 %9 / 10
mrs_uav_managers/src/transform_manager +
82.5%82.5%
+ 		82.5 %354 / 42992.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_trajectory_generation/include/eth_mav_msgs +
80.0%80.0%
+ 		80.0 %24 / 30100.0 %7 / 7
mrs_uav_managers/include/control_manager +
100.0%
+ 		100.0 %4 / 4100.0 %10 / 10
mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly +
91.5%91.5%
+ 		91.5 %364 / 398100.0 %12 / 12
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_lib/src/timer +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
mrs_uav_autostart/src +
87.4%87.4%
+ 		87.4 %284 / 325100.0 %21 / 21
mrs_uav_trajectory_generation/src +
68.2%68.2%
+ 		68.2 %747 / 1095100.0 %22 / 22
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/index-sort-l.html b/index-sort-l.html new file mode 100644 index 0000000000..3c5faf19c7 --- /dev/null +++ b/index-sort-l.html @@ -0,0 +1,662 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:152112365864.3 %
Date:2024-04-09 22:03:33Functions:2680436461.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Directory Sort by nameLine Coverage Sort by line coverageFunctions Sort by function coverage
mrs_lib/include/mrs_lib/safety_zone +
0.0%
+ 		0.0 %0 / 80.0 %0 / 4
mrs_lib/src/scope_timer +
14.1%14.1%
+ 		14.1 %20 / 14250.0 %5 / 10
mrs_uav_trackers/src/speed_tracker +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
mrs_lib/src/geometry +
15.1%15.1%
+ 		15.1 %67 / 44516.0 %15 / 94
mrs_uav_state_estimators/src/estimators/heading +
18.6%18.6%
+ 		18.6 %91 / 49021.1 %12 / 57
mrs_lib/src/profiler +
37.6%37.6%
+ 		37.6 %38 / 10190.0 %9 / 10
mrs_lib/src/safety_zone/polygon +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
mrs_uav_managers/src/control_manager/common +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 31
mrs_uav_trajectory_generation/src/eth_trajectory_generation +
43.6%43.6%
+ 		43.6 %452 / 103629.5 %31 / 105
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.0%48.0%
+ 		48.0 %191 / 39868.4 %13 / 19
mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl +
49.1%49.1%
+ 		49.1 %316 / 64357.6 %19 / 33
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
53.0%53.0%
+ 		53.0 %403 / 76169.5 %66 / 95
mrs_lib/src/transform_broadcaster +
53.3%53.3%
+ 		53.3 %16 / 3066.7 %2 / 3
mrs_uav_state_estimators/src/estimators/altitude +
53.9%53.9%
+ 		53.9 %213 / 39566.7 %22 / 33
mrs_uav_state_estimators/src/estimators/lateral +
55.1%55.1%
+ 		55.1 %241 / 43775.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/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_uav_trajectory_generation/include/eth_trajectory_generation +
62.9%62.9%
+ 		62.9 %129 / 20561.7 %37 / 60
mrs_lib/src/safety_zone +
63.3%63.3%
+ 		63.3 %31 / 4988.9 %8 / 9
mrs_uav_managers/src/control_manager +
64.1%64.1%
+ 		64.1 %2368 / 369472.4 %89 / 123
mrs_lib/src/transformer +
66.1%66.1%
+ 		66.1 %185 / 28076.9 %20 / 26
mrs_uav_managers/src +
66.2%66.2%
+ 		66.2 %1145 / 172978.4 %58 / 74
mrs_uav_trackers/src/line_tracker +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
mrs_uav_trajectory_generation/src +
68.2%68.2%
+ 		68.2 %747 / 1095100.0 %22 / 22
mrs_uav_managers/src/estimation_manager +
70.0%70.0%
+ 		70.0 %433 / 61985.1 %40 / 47
mrs_uav_state_estimators/src/estimators/state +
70.7%70.7%
+ 		70.7 %311 / 44075.7 %28 / 37
mrs_lib/include/mrs_lib/geometry +
71.0%71.0%
+ 		71.0 %66 / 9342.5 %51 / 120
mrs_uav_state_estimators/src/estimators/agl +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors +
72.3%72.3%
+ 		72.3 %133 / 18450.0 %17 / 34
mrs_uav_trackers/src/midair_activation_tracker +
72.8%72.8%
+ 		72.8 %67 / 9250.0 %9 / 18
mrs_uav_trackers/src/landoff_tracker +
73.1%73.1%
+ 		73.1 %433 / 59267.7 %21 / 31
mrs_lib/src/timeout_manager +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
mrs_lib/include/mrs_lib +
79.3%79.3%
+ 		79.3 %781 / 98561.9 %833 / 1345
mrs_uav_trajectory_generation/include/eth_mav_msgs +
80.0%80.0%
+ 		80.0 %24 / 30100.0 %7 / 7
mrs_lib/src/param_loader +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
mrs_uav_trackers/src/mpc_tracker +
80.0%80.0%
+ 		80.0 %1320 / 165080.0 %40 / 50
mrs_uav_controllers/src +
81.0%81.0%
+ 		81.0 %1760 / 217287.9 %58 / 66
mrs_lib/include/mrs_lib/impl +
82.3%82.3%
+ 		82.3 %367 / 44658.4 %806 / 1380
mrs_uav_managers/src/transform_manager +
82.5%82.5%
+ 		82.5 %354 / 42992.9 %13 / 14
mrs_uav_autostart/src +
87.4%87.4%
+ 		87.4 %284 / 325100.0 %21 / 21
mrs_lib/src/median_filter +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly +
91.5%91.5%
+ 		91.5 %364 / 398100.0 %12 / 12
mrs_lib/src/timer +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
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/altitude +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral +
100.0%
+ 		100.0 %10 / 1083.3 %5 / 6
mrs_uav_managers/include/mrs_uav_managers +
100.0%
+ 		100.0 %12 / 1260.0 %6 / 10
mrs_lib/src/math +
100.0%
+ 		100.0 %17 / 17100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/index.html b/index.html new file mode 100644 index 0000000000..7c7ae70f31 --- /dev/null +++ b/index.html @@ -0,0 +1,662 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top levelHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:152112365864.3 %
Date:2024-04-09 22:03:33Functions:2680436461.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Directory Sort by nameLine Coverage Sort by line coverageFunctions Sort by function coverage
mrs_lib/include/mrs_lib +
79.3%79.3%
+ 		79.3 %781 / 98561.9 %833 / 1345
mrs_lib/include/mrs_lib/geometry +
71.0%71.0%
+ 		71.0 %66 / 9342.5 %51 / 120
mrs_lib/include/mrs_lib/impl +
82.3%82.3%
+ 		82.3 %367 / 44658.4 %806 / 1380
mrs_lib/include/mrs_lib/safety_zone +
0.0%
+ 		0.0 %0 / 80.0 %0 / 4
mrs_lib/src/attitude_converter +
94.6%94.6%
+ 		94.6 %212 / 22495.1 %39 / 41
mrs_lib/src/batch_visualizer +
44.1%44.1%
+ 		44.1 %192 / 43544.4 %20 / 45
mrs_lib/src/geometry +
15.1%15.1%
+ 		15.1 %67 / 44516.0 %15 / 94
mrs_lib/src/math +
100.0%
+ 		100.0 %17 / 17100.0 %1 / 1
mrs_lib/src/median_filter +
89.1%89.1%
+ 		89.1 %82 / 9294.1 %16 / 17
mrs_lib/src/param_loader +
80.0%80.0%
+ 		80.0 %44 / 55100.0 %4 / 4
mrs_lib/src/profiler +
37.6%37.6%
+ 		37.6 %38 / 10190.0 %9 / 10
mrs_lib/src/safety_zone +
63.3%63.3%
+ 		63.3 %31 / 4988.9 %8 / 9
mrs_lib/src/safety_zone/polygon +
40.8%40.8%
+ 		40.8 %42 / 10350.0 %4 / 8
mrs_lib/src/scope_timer +
14.1%14.1%
+ 		14.1 %20 / 14250.0 %5 / 10
mrs_lib/src/timeout_manager +
79.0%79.0%
+ 		79.0 %49 / 6281.8 %9 / 11
mrs_lib/src/timer +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
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.4%87.4%
+ 		87.4 %284 / 325100.0 %21 / 21
mrs_uav_controllers/include +
98.3%98.3%
+ 		98.3 %57 / 58100.0 %14 / 14
mrs_uav_controllers/src +
81.0%81.0%
+ 		81.0 %1760 / 217287.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.0%48.0%
+ 		48.0 %191 / 39868.4 %13 / 19
mrs_uav_managers/src +
66.2%66.2%
+ 		66.2 %1145 / 172978.4 %58 / 74
mrs_uav_managers/src/control_manager +
64.1%64.1%
+ 		64.1 %2368 / 369472.4 %89 / 123
mrs_uav_managers/src/control_manager/common +
41.3%41.3%
+ 		41.3 %234 / 56767.7 %21 / 31
mrs_uav_managers/src/estimation_manager +
70.0%70.0%
+ 		70.0 %433 / 61985.1 %40 / 47
mrs_uav_managers/src/estimation_manager/estimators +
60.7%60.7%
+ 		60.7 %82 / 135100.0 %13 / 13
mrs_uav_managers/src/transform_manager +
82.5%82.5%
+ 		82.5 %354 / 42992.9 %13 / 14
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators +
53.0%53.0%
+ 		53.0 %403 / 76169.5 %66 / 95
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl +
100.0%
+ 		100.0 %4 / 4100.0 %3 / 3
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading +
58.3%58.3%
+ 		58.3 %7 / 1257.1 %4 / 7
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral +
100.0%
+ 		100.0 %10 / 1083.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state +
100.0%
+ 		100.0 %9 / 983.3 %5 / 6
mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors +
72.3%72.3%
+ 		72.3 %133 / 18450.0 %17 / 34
mrs_uav_state_estimators/src/estimators/agl +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
mrs_uav_state_estimators/src/estimators/altitude +
53.9%53.9%
+ 		53.9 %213 / 39566.7 %22 / 33
mrs_uav_state_estimators/src/estimators/heading +
18.6%18.6%
+ 		18.6 %91 / 49021.1 %12 / 57
mrs_uav_state_estimators/src/estimators/lateral +
55.1%55.1%
+ 		55.1 %241 / 43775.8 %25 / 33
mrs_uav_state_estimators/src/estimators/state +
70.7%70.7%
+ 		70.7 %311 / 44075.7 %28 / 37
mrs_uav_trackers/src/joy_tracker +
46.5%46.5%
+ 		46.5 %66 / 14238.9 %7 / 18
mrs_uav_trackers/src/landoff_tracker +
73.1%73.1%
+ 		73.1 %433 / 59267.7 %21 / 31
mrs_uav_trackers/src/line_tracker +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
mrs_uav_trackers/src/midair_activation_tracker +
72.8%72.8%
+ 		72.8 %67 / 9250.0 %9 / 18
mrs_uav_trackers/src/mpc_tracker +
80.0%80.0%
+ 		80.0 %1320 / 165080.0 %40 / 50
mrs_uav_trackers/src/speed_tracker +
14.8%14.8%
+ 		14.8 %61 / 41136.8 %7 / 19
mrs_uav_trajectory_generation/include/eth_mav_msgs +
80.0%80.0%
+ 		80.0 %24 / 30100.0 %7 / 7
mrs_uav_trajectory_generation/include/eth_trajectory_generation +
62.9%62.9%
+ 		62.9 %129 / 20561.7 %37 / 60
mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl +
49.1%49.1%
+ 		49.1 %316 / 64357.6 %19 / 33
mrs_uav_trajectory_generation/src +
68.2%68.2%
+ 		68.2 %747 / 1095100.0 %22 / 22
mrs_uav_trajectory_generation/src/eth_trajectory_generation +
43.6%43.6%
+ 		43.6 %452 / 103629.5 %31 / 105
mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly +
91.5%91.5%
+ 		91.5 %364 / 398100.0 %12 / 12
+
+
+ + + + +
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..01755de634 --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.func-sort-c.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/attitude_converter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - attitude_converter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:232785.2 %
Date:2024-04-09 22:03:33Functions: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_Li0EEEIdEEv78671
mrs_lib::Vector3Converter::Vector3Converter(tf2::Vector3 const&)251514
mrs_lib::AttitudeConverter::AttitudeConverter<double>(Eigen::AngleAxis<double>)525254
+
+
+ + + +
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..abe4d3e535 --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.func.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/attitude_converter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - attitude_converter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:232785.2 %
Date:2024-04-09 22:03:33Functions: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&)251514
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>)525254
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_Li0EEEIdEEv78671
_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..4cd0cd3a25 --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.html @@ -0,0 +1,617 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/attitude_converter.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - attitude_converter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:232785.2 %
Date:2024-04-09 22:03:33Functions: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      251514 :   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    11829123 :   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      525254 :   AttitudeConverter(const Eigen::AngleAxis<T> angle_axis) {
+     254      522697 :     double       angle = angle_axis.angle();
+     255      523536 :     tf2::Vector3 axis(angle_axis.axis()[0], angle_axis.axis()[1], angle_axis.axis()[2]);
+     256             : 
+     257      524575 :     tf2_quaternion_.setRotation(axis, angle);
+     258      523927 :   }
+     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       78671 :   operator Eigen::Quaternion<T>() const {
+     334             : 
+     335       78671 :     return Eigen::Quaternion<T>(tf2_quaternion_.w(), tf2_quaternion_.x(), tf2_quaternion_.y(), tf2_quaternion_.z());
+     336             :   }
+     337             : 
+     338             :   operator Eigen::Matrix3d() const;
+     339             : 
+     340             :   /**
+     341             :    * @brief typecase to tuple of Euler angles in extrinsic RPY
+     342             :    *
+     343             :    * @return std::tuple of extrinsic RPY
+     344             :    */
+     345             :   operator std::tuple<double&, double&, double&>();
+     346             : 
+     347             :   /**
+     348             :    * @brief typecase to tf2::Matrix3x3
+     349             :    *
+     350             :    * @return tf2::Matrix3x3 rotational matrix
+     351             :    */
+     352             :   operator tf2::Matrix3x3() const;
+     353             : 
+     354             :   /**
+     355             :    * @brief typecase to tf2::Transform
+     356             :    *
+     357             :    * @return tf2::Transform
+     358             :    */
+     359             :   operator tf2::Transform() const;
+     360             : 
+     361             :   //}
+     362             : 
+     363             :   /* getters //{ */
+     364             : 
+     365             :   /**
+     366             :    * @brief get the roll angle
+     367             :    *
+     368             :    * @return roll
+     369             :    */
+     370             :   double getRoll(void);
+     371             : 
+     372             :   /**
+     373             :    * @brief get the pitch angle
+     374             :    *
+     375             :    * @return pitch
+     376             :    */
+     377             :   double getPitch(void);
+     378             : 
+     379             :   /**
+     380             :    * @brief get the yaw angle
+     381             :    *
+     382             :    * @return yaw
+     383             :    */
+     384             :   double getYaw(void);
+     385             : 
+     386             :   /**
+     387             :    * @brief get the angle of the rotated x-axis in the original XY plane, a.k.a
+     388             :    *
+     389             :    * @return heading
+     390             :    */
+     391             :   double getHeading(void);
+     392             : 
+     393             :   /**
+     394             :    * @brief get heading rate base on the orientation and body-based attitude rate
+     395             :    *
+     396             :    * @param attitude_rate in the body frame
+     397             :    *
+     398             :    * @return heading rate in the world
+     399             :    */
+     400             :   double getHeadingRate(const Vector3Converter& attitude_rate);
+     401             : 
+     402             :   /**
+     403             :    * @brief get the intrinsic yaw rate from a heading rate
+     404             :    *
+     405             :    * @param heading_rate
+     406             :    *
+     407             :    * @return intrinsic yaw rate
+     408             :    */
+     409             :   double getYawRateIntrinsic(const double& heading_rate);
+     410             : 
+     411             :   /**
+     412             :    * @brief get a unit vector pointing in the X direction
+     413             :    *
+     414             :    * @return the vector
+     415             :    */
+     416             :   Vector3Converter getVectorX(void);
+     417             : 
+     418             :   /**
+     419             :    * @brief get a unit vector pointing in the Y direction
+     420             :    *
+     421             :    * @return the vector
+     422             :    */
+     423             :   Vector3Converter getVectorY(void);
+     424             : 
+     425             :   /**
+     426             :    * @brief get a unit vector pointing in the Z direction
+     427             :    *
+     428             :    * @return the vector
+     429             :    */
+     430             :   Vector3Converter getVectorZ(void);
+     431             : 
+     432             :   /**
+     433             :    * @brief get the Roll, Pitch, Yaw angles in the Intrinsic convention
+     434             :    *
+     435             :    * @return RPY
+     436             :    */
+     437             :   std::tuple<double, double, double> getIntrinsicRPY();
+     438             : 
+     439             :   /**
+     440             :    * @brief get the Roll, Pitch, Yaw angles in the Extrinsic convention. The same as the default AttitudeConverter assignment.
+     441             :    *
+     442             :    * @return RPY
+     443             :    */
+     444             :   std::tuple<double, double, double> getExtrinsicRPY();
+     445             : 
+     446             :   //}
+     447             : 
+     448             :   /* setters //{ */
+     449             : 
+     450             :   /**
+     451             :    * @brief Updates the heading of the current orientation by updating the intrinsic yaw
+     452             :    *
+     453             :    * @param new heading
+     454             :    *
+     455             :    * @return the orientation
+     456             :    */
+     457             :   AttitudeConverter setHeading(const double& heading);
+     458             : 
+     459             :   /**
+     460             :    * @brief Updates the extrinsic yaw of the current orientation.
+     461             :    *
+     462             :    * @param new yaw
+     463             :    *
+     464             :    * @return the orientation
+     465             :    */
+     466             :   AttitudeConverter setYaw(const double& new_yaw);
+     467             : 
+     468             :   //}
+     469             : 
+     470             :   template <std::size_t I>
+     471             :   constexpr auto get();
+     472             : 
+     473             : private:
+     474             :   /**
+     475             :    * @brief Internal representation of the attitude
+     476             :    */
+     477             :   tf2::Quaternion tf2_quaternion_;
+     478             : 
+     479             :   /**
+     480             :    * @brief convert the internal quaternion representation to internally-stored RPY
+     481             :    */
+     482             :   void calculateRPY(void);
+     483             : 
+     484             :   /**
+     485             :    * @brief throws exception when the internal attitude is invalid
+     486             :    */
+     487             :   void validateOrientation(void);
+     488             : 
+     489             :   /**
+     490             :    * @brief Internal representation in RPY. is used only when converting to RPY.
+     491             :    */
+     492             :   double roll_, pitch_, yaw_;
+     493             :   bool   got_rpy_ = false;
+     494             : };
+     495             : 
+     496             : 
+     497             : template <std::size_t I>
+     498           3 : constexpr auto AttitudeConverter::get() {
+     499             : 
+     500           3 :   calculateRPY();
+     501             : 
+     502             :   // call compilation error if I > 2
+     503             :   static_assert(I <= 2);
+     504             : 
+     505             :   // get the RPY components based on the index in the tuple
+     506             :   if constexpr (I == 0) {
+     507           1 :     return static_cast<double>(roll_);
+     508             :   } else if constexpr (I == 1) {
+     509           1 :     return static_cast<double>(pitch_);
+     510             :   } else if constexpr (I == 2) {
+     511           1 :     return static_cast<double>(yaw_);
+     512             :   }
+     513             : }
+     514             : 
+     515             : }  // namespace mrs_lib
+     516             : 
+     517             : template <>
+     518             : struct std::tuple_size<mrs_lib::AttitudeConverter>
+     519             : { static constexpr int value = 3; };
+     520             : 
+     521             : template <>
+     522             : struct std::tuple_element<0, mrs_lib::AttitudeConverter>
+     523             : { using type = double; };
+     524             : 
+     525             : template <>
+     526             : struct std::tuple_element<1, mrs_lib::AttitudeConverter>
+     527             : { using type = double; };
+     528             : 
+     529             : template <>
+     530             : struct std::tuple_element<2, mrs_lib::AttitudeConverter>
+     531             : { using type = double; };
+     532             : 
+     533             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.overview.html b/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.overview.html new file mode 100644 index 0000000000..2b64ca38c0 --- /dev/null +++ b/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.overview.html @@ -0,0 +1,154 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/attitude_converter.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.png b/mrs_lib/include/mrs_lib/attitude_converter.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..5645ea2bb184ba5c13faadff6ae080a7ed21a006 GIT binary patch literal 1675 zcmV;626Xv}P)R0{{R3v^8aD0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp^O~xJw}!I*vg`@baJIpOoIavV`$(K?>!`bo|a^C) za8g(t(GB3SHwi*Y*B(ABA)4cq7R?deK-XjKEeJ~&vr9LR}k6~|SE zj-$qCkGRR)tetw3kCxx*!;6UEC@RHf4BS;>N2h>L$)pCW@@QNm!OX>*`BO)Lk71D%>OnE>K^hjQTBMES? zGvhv}xdH-50z4|BgOy^!kpa(GH^`~foRA8S^Y>UW#vJMJOe}{A4}y>ij~qH^H4MF) zneqsxd6A=h9u5{hcDe;P(zMKk1rd-Ue&^knPPR3jA9^gb>#8x~Ik7N~R1Fdq@=|ju z0T!S8M{bY&wr%zW06bfshHfN{MDlGm(rkM>62Wo>ikiawavEnFLStBy1HdIWEFe%Ww^Ngwm;Hcq)jl z0*{0W;oTtCZ~llxle+lG>3#O+D&&gJrG@=)lao1h2seK7gq0SK9kjuxg-NrKGR!%~ z`au99dU_T5N&6~=-sZqq>k<~8`<@qW?#HHsHY0qrv%R~ejG3dmdiYQNZF4O4*G{-$ z^G7N8QUf@SO&a1DH?d4h1F$$gpRd=K`+0r3pV#NJ)po+CrwQ%T-SgEVtiNn>JQv4Q z$!a>`qaW=;`3?H`0rb(Ur2TajeM}|3XxAlSQK}p$B;JRoy~&s`4k)2hNq1Dm2(-&Z z%ZP0UeT*pTteufi+CMJX^?uWrH*Wvgee-&=U5j%JyB%`DkeX;_W9brNyMAW@LV=NF z`vbMAFhtINPYBIhx=Oy)`A1x1t=K11!#H_Sq=yYc#`?Mxm2Dy{cz^))?R~`tEg7 zo(>_l>s;xc@cdbu``xNXAjEdvf~QT$fk&n`v;a?Mx|iT-@?_UO)>ViPPuH7ayKcbK zIe5W=N5r<4w5LOe?HZq((Kwy}I4+3;`PQSeHB3yo++}$V=9qWo+^i(35sOuwsIM>rafH+-<*caYfll7-k8qBGOLd1CnUsn zot2`+1`_h(bt-s#>%p`$vc0^nk+7M>5i7}n<3->Z68->q@ErZ91s_QKmCsa&B%_`q z1D@+YQz7;U=C6FFLhb=op7xZL`xqZRZtpz_n1U--iU~*Zp}2TpDV}{(xy0affm7%5 z+W7_^4dR!LZX*O5l2^DZ1 z?A~WeTGw4u;hC=z$ot`PUf>U&F~>i6h8%ghnJNAcf#T%Hdy|jS=_H)EsjMJ8?!*5I z51!+@7iT$1QUD{R;|$UhBs~Y1;Zi2XPgaWJl!jXIElj|TK46~NkO7}ca2>#x;XcI* z^CV0j8!}-zAteJI3FZ@GU`1FF5~pzC_+7=3X)_ASQIrJ;gUR_iSNsD<>R>{U1Y^?W zvYR!Sg}4SeuBe%R`Cnmz$V;iPVjSJHb9JE2c3kC8_@f)zqlD5AEDo&}n_5NIC62c= zqkgYRb;F6tx*!Fl3jb2?u8vgi2?nd`J-wb + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - dynamic_reconfigure_mgr.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:578765.5 %
Date:2024-04-09 22:03:33Functions: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&)85
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::LateralEstimatorConfig const&)85
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)85
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&)85
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::load_defaults(mrs_uav_state_estimators::AltitudeEstimatorConfig&)146
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)146
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)146
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&)146
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::update_config(mrs_uav_state_estimators::LateralEstimatorConfig const&)170
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&)255
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&)255
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<double>(boost::any&, double*&)255
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<int>(boost::any&, int*&)255
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::update_config(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)292
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&)304
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&)304
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::load_defaults(mrs_uav_state_estimators::CorrectionConfig&)304
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_changed_params(mrs_uav_state_estimators::CorrectionConfig const&)304
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::CorrectionConfig&, unsigned int)304
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<double>(boost::any&, double*&)304
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<int>(boost::any&, int*&)304
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)304
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&)304
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&)438
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&)438
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<double>(boost::any&, double*&)438
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<int>(boost::any&, int*&)438
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::update_config(mrs_uav_state_estimators::CorrectionConfig const&)608
+
+
+ + + +
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..f1418ee6f9 --- /dev/null +++ b/mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.func.html @@ -0,0 +1,420 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - dynamic_reconfigure_mgr.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:578765.5 %
Date:2024-04-09 22:03:33Functions: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&)304
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&)304
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&)304
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::update_config(mrs_uav_state_estimators::CorrectionConfig const&)608
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::print_changed_params(mrs_uav_state_estimators::CorrectionConfig const&)304
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::CorrectionConfig&, unsigned int)304
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*&)304
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::try_cast<int>(boost::any&, int*&)304
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::CorrectionConfig>::DynamicReconfigureMgr(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)304
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&)304
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&)255
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&)255
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&)85
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::update_config(mrs_uav_state_estimators::LateralEstimatorConfig const&)170
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::LateralEstimatorConfig const&)85
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::LateralEstimatorConfig&, unsigned int)85
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*&)255
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::LateralEstimatorConfig>::try_cast<int>(boost::any&, int*&)255
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&)85
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&)438
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&)438
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&)146
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::update_config(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)292
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::print_changed_params(mrs_uav_state_estimators::AltitudeEstimatorConfig const&)146
mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::dynamic_reconfigure_callback(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)146
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*&)438
bool mrs_lib::DynamicReconfigureMgr<mrs_uav_state_estimators::AltitudeEstimatorConfig>::try_cast<int>(boost::any&, int*&)438
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&)146
+
+
+ + + +
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..a18747856a --- /dev/null +++ b/mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h.gcov.html @@ -0,0 +1,343 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/dynamic_reconfigure_mgr.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - dynamic_reconfigure_mgr.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:578765.5 %
Date:2024-04-09 22:03:33Functions: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         535 :   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         535 :         m_server(m_server_mtx, nh),
+      54             :         m_usr_cbf(user_callback),
+      55         535 :         m_pl(nh, print_values, node_name)
+      56             :   {
+      57             :     // initialize the dynamic reconfigure callback
+      58         535 :     m_server.setCallback(boost::bind(&DynamicReconfigureMgr<ConfigType>::dynamic_reconfigure_callback, this, _1, _2));
+      59         535 :   };
+      60             : 
+      61             :   /* Constructor overloads //{ */
+      62             :   // Convenience constructor to enable writing DynamicReconfigureMgr<MyConfig> drmgr(nh, node_name)
+      63         304 :   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        1070 :   void update_config(const ConfigType& cfg)
+      72             :   {
+      73        1070 :     m_server.updateConfig(cfg);
+      74        1070 :   }
+      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         535 :   void dynamic_reconfigure_callback(ConfigType& new_config, uint32_t level)
+     107             :   {
+     108         535 :     if (m_print_values)
+     109             :     {
+     110         535 :       if (m_node_name.empty())
+     111           0 :         ROS_INFO("Dynamic reconfigure request received");
+     112             :       else
+     113         535 :         ROS_INFO("[%s]: Dynamic reconfigure request received", m_node_name.c_str());
+     114             :     }
+     115             : 
+     116         535 :     if (m_not_initialized)
+     117             :     {
+     118         535 :       load_defaults(new_config);
+     119         535 :       update_config(new_config);
+     120             :     }
+     121         535 :     if (m_print_values)
+     122             :     {
+     123         535 :       print_changed_params(new_config);
+     124             :     }
+     125         535 :     m_not_initialized = false;
+     126         535 :     config = new_config;
+     127         535 :     if (m_usr_cbf)
+     128         231 :       m_usr_cbf(new_config, level);
+     129         535 :   }
+     130             : 
+     131             :   template <typename T>
+     132         997 :   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        1994 :     boost::shared_ptr<const param_descr_t> cast_descr = boost::dynamic_pointer_cast<const param_descr_t>(descr);
+     136         997 :     m_pl.loadParam(name, config.*(cast_descr->field));
+     137         997 :   }
+     138             :   
+     139         535 :   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        1070 :     std::vector<typename ConfigType::AbstractParamDescriptionConstPtr> descrs = new_config.__getParamDescriptions__();
+     143        1532 :     for (auto& descr : descrs)
+     144             :     {
+     145        1994 :       std::string name = descr->name;
+     146         997 :       size_t pos = name.find("__");
+     147         997 :       while (pos != name.npos)
+     148             :       {
+     149           0 :         name.replace(pos, 2, "/");
+     150           0 :         pos = name.find("__");
+     151             :       }
+     152             : 
+     153         997 :       if (descr->type == "bool")
+     154           0 :         load_param<bool>(name, descr, new_config);
+     155         997 :       else if (descr->type == "int")
+     156           0 :         load_param<int>(name, descr, new_config);
+     157         997 :       else if (descr->type == "double")
+     158         997 :         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         535 :   }
+     168             : 
+     169             :   // method for printing names and values of new received parameters (prints only the changed ones) //{
+     170         535 :   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        1070 :     std::vector<typename ConfigType::AbstractParamDescriptionConstPtr> descrs = new_config.__getParamDescriptions__();
+     174        1532 :     for (auto& descr : descrs)
+     175             :     {
+     176           0 :       boost::any val, old_val;
+     177         997 :       descr->getValue(new_config, val);
+     178         997 :       descr->getValue(config, old_val);
+     179         997 :       std::string name = descr->name;
+     180         997 :       const size_t pos = name.find("__");
+     181         997 :       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         997 :       if (try_cast(val, intval))
+     199             :       {
+     200           0 :         if (m_not_initialized || !try_compare(old_val, intval))
+     201           0 :           print_value(name, *intval);
+     202         997 :       } else if (try_cast(val, doubleval))
+     203             :       {
+     204         997 :         if (m_not_initialized || !try_compare(old_val, doubleval))
+     205         997 :           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         535 :   }
+     220             :   //}
+     221             :   
+     222             :   // helper method for parameter printing
+     223             :   template <typename T>
+     224         997 :   inline void print_value(const std::string& name, const T& val)
+     225             :   {
+     226         997 :     if (m_node_name.empty())
+     227           0 :       std::cout << "\t" << name << ":\t" << val << std::endl;
+     228             :     else
+     229         997 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << name << "':\t" << val);
+     230         997 :   }
+     231             :   // helper methods for automatic parameter value parsing
+     232             :   template <typename T>
+     233        1994 :   inline bool try_cast(boost::any& val, T*& out)
+     234             :   {
+     235        1994 :     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..1eeec2f074 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/cyclic.h.func-sort-c.html @@ -0,0 +1,548 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/cyclic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - cyclic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:608769.0 %
Date:2024-04-09 22:03:33Functions:4811741.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::degrees const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::dist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic(mrs_lib::geometry::sdegrees const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(mrs_lib::geometry::sradians const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::value() const0
mrs_lib::geometry::radians mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::convert<mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)1
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)1
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)1
mrs_lib::geometry::radians mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::convert<mrs_lib::geometry::radians>() const1
bool mrs_lib::geometry::operator><double, mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)2
bool mrs_lib::geometry::operator><double, mrs_lib::geometry::sradians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)2
bool mrs_lib::geometry::operator< <double, mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)2
bool mrs_lib::geometry::operator< <double, mrs_lib::geometry::sradians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)2
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::value() const4
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>&&)6
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(double, double, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(double, double, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::unwrap(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::diff(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::unwrap(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::dist(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::unwrap(double, double)10001
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::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>)10008
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::radians>::dist(double, double)10374
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::cyclic(double)20000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(double)20005
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sdegrees>::wrap(double)30000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::wrap(double)30005
mrs_lib::geometry::operator+(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)40000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::value() const40002
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)361521
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(double, double)413924
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)517800
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interpUnwrapped(double, double, double)517800
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interp(double, double, double)517800
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::radians const&)701463
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)744394
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>)786314
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians> const&)1055624
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(double)1602255
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::wrap(double)1624760
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::unwrap(double, double)2621209
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::diff(double, double)2634780
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>)3162588
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(double)6425173
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::wrap(double)7052677
+
+
+ + + +
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..12dfa52efa --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/cyclic.h.func.html @@ -0,0 +1,548 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/cyclic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - cyclic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:608769.0 %
Date:2024-04-09 22:03:33Functions:4811741.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::diff(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::dist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::wrap(double)30005
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::interp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::unwrap(double, double)10000
mrs_lib::geometry::radians mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::convert<mrs_lib::geometry::radians>(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)1
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::degrees const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic(double)20005
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::cyclic()0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>&&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator=(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator-=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees>::operator+=(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::degrees> const&)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interpUnwrapped(double, double, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterpUnwrapped(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)786314
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::diff(double, double)413924
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>)361521
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::dist(double, double)10374
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::wrap(double)1624760
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pdist(double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::interp(double, double, double)898
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::unwrap(double, double)10001
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::inRange(double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::pinterp(double, double, double)0
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::radians const&)701463
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians> const&)744394
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::radians>::cyclic(double)1602255
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)517800
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::interpUnwrapped(double, double, double)517800
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>)3162588
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::diff(double, double)2634780
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>)10008
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::dist(double, double)10000
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::wrap(double)7052677
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)517800
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::unwrap(double, double)2621209
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&)1055624
mrs_lib::geometry::cyclic<double, mrs_lib::geometry::sradians>::cyclic(double)6425173
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..d0eefacd0a --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.html @@ -0,0 +1,612 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/cyclic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - cyclic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:608769.0 %
Date:2024-04-09 22:03:33Functions: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     8067433 :       cyclic(const flt val) : val(wrap(val)){};
+      62             :       /*!
+      63             :        * \brief Copy constructor.
+      64             :        *
+      65             :        * \param val initialization value.
+      66             :        */
+      67     1800018 :       cyclic(const cyclic& other) : val(other.val){};
+      68             :       /*!
+      69             :        * \brief Copy constructor.
+      70             :        *
+      71             :        * \param val initialization value.
+      72             :        */
+      73      701463 :       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     8737442 :       static flt wrap(const flt val)
+     115             :       {
+     116             :         // these few ifs should cover most cases, improving speed and precision
+     117     8737442 :         if (val >= minimum)
+     118             :         {
+     119     8079915 :           if (val < supremum)  // value is actually in range and doesn't need to be wrapped
+     120     7850227 :             return val;
+     121      229687 :           else if (val < supremum + range)
+     122       67557 :             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      657532 :           if (val >= minimum - range)
+     126      493251 :             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      326489 :         const flt rem = std::fmod(val - minimum, range);
+     131      326489 :         const flt wrapped = rem + minimum + std::signbit(rem) * range;
+     132      326402 :         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     2651210 :       static flt unwrap(const flt what, const flt from)
+     158             :       {
+     159     2651210 :         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     3068704 :       static flt diff(const flt minuend, const flt subtrahend)
+     194             :       {
+     195     3068704 :         return diff(cyclic(minuend), cyclic(subtrahend));
+     196             :       }
+     197             : 
+     198     3968902 :       static flt diff(const cyclic minuend, const cyclic subtrahend)
+     199             :       {
+     200     3968902 :         const flt d = minuend.val - subtrahend.val;
+     201     3968902 :         if (d < -half_range)
+     202       34958 :           return d + range;
+     203     3933944 :         if (d >= half_range)
+     204       27453 :           return d - range;
+     205     3906491 :         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       20374 :       static flt dist(const flt from, const flt to)
+     220             :       {
+     221       20374 :         return dist(cyclic(from), cyclic(to));
+     222             :       }
+     223             : 
+     224      371529 :       static flt dist(const cyclic from, const cyclic to)
+     225             :       {
+     226      371529 :         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      518698 :       static flt interpUnwrapped(const flt from, const flt to, const flt coeff)
+     243             :       {
+     244      518698 :         return interpUnwrapped(cyclic(from), cyclic(to), coeff);
+     245             :       }
+     246             : 
+     247      518698 :       static flt interpUnwrapped(const cyclic from, const cyclic to, const flt coeff)
+     248             :       {
+     249      518698 :         const flt dang = diff(to, from);
+     250      518698 :         const flt intp = from.val + coeff * dang;
+     251      518698 :         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      518698 :       static flt interp(const flt from, const flt to, const flt coeff)
+     265             :       {
+     266      518698 :         return wrap(interpUnwrapped(from, to, coeff));
+     267             :       }
+     268             : 
+     269           0 :       static flt interp(const cyclic from, const cyclic to, const flt coeff)
+     270             :       {
+     271           0 :         return wrap(interpUnwrapped(from, to, coeff));
+     272             :       }
+     273             : 
+     274             :       /*!
+     275             :        * \brief Interpolation between two circular quantities in the positive direction without wrapping of the result.
+     276             :        *
+     277             :        * Interpolates the two values in the positive direction from the first parameter to the second by the coefficient.
+     278             :        * This function doesn't wrap the returned value.
+     279             :        *
+     280             :        * \param from  the first circular quantity.
+     281             :        * \param to    the second circular quantity.
+     282             :        * \param coeff the interpolation coefficient.
+     283             :        * \returns     interpolation of the two circular quantities using the coefficient.
+     284             :        *
+     285             :        * \warning Note that the returned value may be outside the valid interval of wrapped values, specified by the \p minimum and \p supremum parameters of
+     286             :        * this class.
+     287             :        */
+     288           0 :       static flt pinterpUnwrapped(const flt from, const flt to, const flt coeff)
+     289             :       {
+     290           0 :         return pinterpUnwrapped(cyclic(from), cyclic(to), coeff);
+     291             :       }
+     292             : 
+     293           0 :       static flt pinterpUnwrapped(const cyclic from, const cyclic to, const flt coeff)
+     294             :       {
+     295           0 :         const flt dang = pdist(to, from);
+     296           0 :         const flt intp = from.val + coeff * dang;
+     297           0 :         return intp;
+     298             :       }
+     299             : 
+     300             :       /*!
+     301             :        * \brief Interpolation between two circular quantities in the positive direction.
+     302             :        *
+     303             :        * Interpolates the two values in the positive direction from the first parameter to the second by the coefficient.
+     304             :        * This function wraps the returned value so that it is in the interval of valid values.
+     305             :        *
+     306             :        * \param from  the first circular quantity.
+     307             :        * \param to    the second circular quantity.
+     308             :        * \param coeff the interpolation coefficient.
+     309             :        * \returns     interpolation of the two circular quantities using the coefficient, wrapped to the interval of valid values.
+     310             :        */
+     311           0 :       static flt pinterp(const flt from, const flt to, const flt coeff)
+     312             :       {
+     313           0 :         return pinterpUnwrapped(cyclic(from), cyclic(to), coeff);
+     314             :       }
+     315             : 
+     316           0 :       static flt pinterp(const cyclic from, const cyclic to, const flt coeff)
+     317             :       {
+     318           0 :         return wrap(pinterpUnwrapped(from, to, coeff));
+     319             :       }
+     320             : 
+     321             :       /*!
+     322             :        * \brief Conversion between two different circular quantities.
+     323             :        *
+     324             :        * This function converts its parameter, interpreted as the circular quantity, represented by this class, to the \p other_t type of circular quantity.
+     325             :        *
+     326             :        * \param what       the circular quantity to be converted.
+     327             :        * \returns          the circular quantity converted to the range of \p other_t.
+     328             :        * \tparam other_t   type of the circular quantity to be converted to.
+     329             :        *
+     330             :        * \warning For the purposes of this function, it is assumed that the range of one type corresponds to the whole range of the other type and zeros of both
+     331             :        * types correspond to each other (such as when converting eg. degrees to radians).
+     332             :        */
+     333             :       template <class other_t>
+     334           1 :       static other_t convert(const cyclic& what)
+     335             :       {
+     336           1 :         return other_t(what.val / range * other_t::range);
+     337             :       }
+     338             : 
+     339             :       /*!
+     340             :        * \brief Conversion between two different circular quantities.
+     341             :        *
+     342             :        * This method returns the circular quantity, represented by this object, converted to the \p other_t type of circular quantity.
+     343             :        *
+     344             :        * \returns          the circular quantity converted to the range of \p other_t.
+     345             :        * \tparam other_t   type of the circular quantity to be converted to.
+     346             :        *
+     347             :        * \warning For the purposes of this function, it is assumed that the range of one type corresponds to the whole range of the other type and zeros of both
+     348             :        * types correspond to each other (such as when converting eg. degrees to radians).
+     349             :        */
+     350             :       template <class other_t>
+     351           1 :       other_t convert() const
+     352             :       {
+     353           1 :         return other_t(val / range * other_t::range);
+     354             :       }
+     355             : 
+     356             :       // | ------------------------ Operators ----------------------- |
+     357             :       /*!
+     358             :        * \brief Assignment operator.
+     359             :        *
+     360             :        * \param nval value to be assigned (will be wrapped).
+     361             :        * \return     reference to self.
+     362             :        */
+     363           0 :       cyclic& operator=(const flt nval)
+     364             :       {
+     365           0 :         val = wrap(nval);
+     366           0 :         return *this;
+     367             :       };
+     368             :       /*!
+     369             :        * \brief Assignment operator.
+     370             :        *
+     371             :        * \param other value to be assigned.
+     372             :        * \return      reference to self.
+     373             :        */
+     374           0 :       cyclic& operator=(const cyclic& other)
+     375             :       {
+     376           0 :         val = other.val;
+     377           0 :         return *this;
+     378             :       };
+     379             :       /*!
+     380             :        * \brief Move operator.
+     381             :        *
+     382             :        * \param other value to be assigned.
+     383             :        * \return      reference to self.
+     384             :        */
+     385           6 :       cyclic& operator=(cyclic&& other)
+     386             :       {
+     387           6 :         val = other.val;
+     388           6 :         return *this;
+     389             :       };
+     390             : 
+     391             :       /*!
+     392             :        * \brief Addition compound operator.
+     393             :        *
+     394             :        * \param other value to be added.
+     395             :        * \return      reference to self.
+     396             :        */
+     397           1 :       cyclic& operator+=(const cyclic& other)
+     398             :       {
+     399           1 :         val = wrap(val + other.val);
+     400           1 :         return *this;
+     401             :       };
+     402             : 
+     403             :       /*!
+     404             :        * \brief Subtraction compound operator.
+     405             :        *
+     406             :        * \param other value to be subtracted.
+     407             :        * \return      reference to self.
+     408             :        */
+     409           1 :       cyclic& operator-=(const cyclic& other)
+     410             :       {
+     411           1 :         val = diff(val, other.val);
+     412           1 :         return *this;
+     413             :       };
+     414             : 
+     415             :       /*!
+     416             :        * \brief Addition operator.
+     417             :        *
+     418             :        * \param lhs left-hand-side.
+     419             :        * \param rhs right-hand-side.
+     420             :        * \return    the result of adding the two angles.
+     421             :        */
+     422       50004 :       friend spec operator+(const cyclic& lhs, const cyclic& rhs)
+     423             :       {
+     424       50004 :         return wrap(lhs.val + rhs.val);
+     425             :       }
+     426             : 
+     427             :       /*!
+     428             :        * \brief Subtraction operator (uses the diff() method).
+     429             :        *
+     430             :        * \param lhs left-hand-side.
+     431             :        * \param rhs right-hand-side.
+     432             :        * \return    the result of subtracting rhs from lhs.
+     433             :        */
+     434       10009 :       friend flt operator-(const cyclic& lhs, const cyclic& rhs)
+     435             :       {
+     436       10009 :         return diff(lhs, rhs);
+     437             :       }
+     438             : 
+     439             :       protected:
+     440             :         flt val;
+     441             :       };
+     442             : 
+     443             :     /*!
+     444             :      * \brief Implementation of the comparison operation between two angles.
+     445             :      *
+     446             :      * An angle is considered to be smaller than another angle if it is shorter - closer to zero.
+     447             :      *
+     448             :      * \param lhs left-hand-side.
+     449             :      * \param rhs right-hand-side.
+     450             :      * \return    true iff the shortest unsigned walk from lhs to 0 is less than from rhs to 0.
+     451             :      */
+     452             :     template <typename flt, class spec>
+     453           4 :     bool operator<(const cyclic<flt, spec>& lhs, const cyclic<flt, spec>& rhs)
+     454             :     {
+     455           4 :       return cyclic<flt, spec>::dist(lhs, 0) < cyclic<flt, spec>::dist(rhs, 0);
+     456             :     }
+     457             : 
+     458             :     /*!
+     459             :      * \brief Implementation of the comparison operation between two angles.
+     460             :      *
+     461             :      * An angle is considered to be larger than another angle if it is longer - further from zero.
+     462             :      *
+     463             :      * \param lhs left-hand-side.
+     464             :      * \param rhs right-hand-side.
+     465             :      * \return    true iff the shortest unsigned walk from lhs to 0 is more than from rhs to 0.
+     466             :      */
+     467             :     template <typename flt, class spec>
+     468           4 :     bool operator>(const cyclic<flt, spec>& lhs, const cyclic<flt, spec>& rhs)
+     469             :     {
+     470           4 :       return cyclic<flt, spec>::dist(lhs, 0) > cyclic<flt, spec>::dist(rhs, 0);
+     471             :     }
+     472             : 
+     473             :     /*!
+     474             :      * \brief Implementation of the stream output operator.
+     475             :      *
+     476             :      * \param out the stream to write the angle to.
+     477             :      * \param ang the angle to be written.
+     478             :      * \return    a reference to the stream.
+     479             :      */
+     480             :     template <typename flt, class spec>
+     481             :     std::ostream& operator<<(std::ostream &out, const cyclic<flt, spec>& ang)
+     482             :     {
+     483             :       return (out << ang.value());
+     484             :     }
+     485             : 
+     486             :     /*!
+     487             :      * \brief Convenience specialization of the cyclic class for unsigned radians (from $0$ to $2\pi$).
+     488             :      */
+     489             :     struct radians : public cyclic<double, radians>
+     490             :     {
+     491             :       using cyclic<double, radians>::cyclic;  // necessary to inherit constructors
+     492             :       static constexpr double minimum = 0;
+     493             :       static constexpr double supremum = 2 * M_PI;
+     494             :     };
+     495             : 
+     496             :     /*!
+     497             :      * \brief Convenience specialization of the cyclic class for signed radians (from $-\pi$ to $\pi$).
+     498             :      */
+     499             :     struct sradians : public cyclic<double, sradians>
+     500             :     {
+     501             :       using cyclic<double, sradians>::cyclic;  // necessary to inherit constructors
+     502             :       static constexpr double minimum = -M_PI;
+     503             :       static constexpr double supremum = M_PI;
+     504             :     };
+     505             : 
+     506             :     /*!
+     507             :      * \brief Convenience specialization of the cyclic class for unsigned degrees (from $0$ to $360$).
+     508             :      */
+     509             :     struct degrees : public cyclic<double, degrees>
+     510             :     {
+     511             :       using cyclic<double, degrees>::cyclic;  // necessary to inherit constructors
+     512             :       static constexpr double minimum = 0;
+     513             :       static constexpr double supremum = 360;
+     514             :     };
+     515             : 
+     516             :     /*!
+     517             :      * \brief Convenience specialization of the cyclic class for signed degrees (from $-180$ to $180$).
+     518             :      */
+     519             :     struct sdegrees : public cyclic<double, sdegrees>
+     520             :     {
+     521             :       using cyclic<double, sdegrees>::cyclic;  // necessary to inherit constructors
+     522             :       static constexpr double minimum = -180;
+     523             :       static constexpr double supremum = 180;
+     524             :     };
+     525             :   }  // namespace geometry
+     526             : }  // namespace mrs_lib
+     527             : 
+     528             : #endif  // CYCLIC_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.overview.html b/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.overview.html new file mode 100644 index 0000000000..1ad3dfadb9 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.overview.html @@ -0,0 +1,152 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/cyclic.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.png b/mrs_lib/include/mrs_lib/geometry/cyclic.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..966598f52a04aad7097c45178d084b601fee1a23 GIT binary patch literal 2145 zcmV-n2%h(eP)ztac=LGdqZvm__Cy*;dNOmM>B;1gHpm39@cZ%O zo?OEbB%EeIK<43HR=#8OfIF6AR0ZMg;XeTAM1kZCYC>dY$_&f0mBhxF z5qxarj^u6udxMlZGR2bG2SY}qvfda;4-0{@P+@~j6|ifMpP!#!&-HkG;rgsB0NiTt zT0p}!`|BxObM#`*CTrax@8kO8$8lhu>l)T!uHm=F z`2Uyd2GhWG*NRAo0{{ls7GNhr6G;#D&@Y}SlC@y|d-3$x1j-S_eE<%mdj;OTG%L8k zwPqAmlHhYdX_HJVg*9#p*j7_ul7hu+X7*BerKF#1) znkZ)+m*NHUUdB`?wR+H&Ps$r0#H71=?Dts@ZPiOF>vsDYNk})y!q2e#47{&K+_8h5pxxg zM0rEnnBu9rq2n7eQl0#!1%2KcK9#@MhxjYrLq0U9(mrv!z#yvFwSBG*wV1UIG*A#Vyyf+7XT!r zP{65$-0OrCp5W>Pj`J}9_L9!5$7%Uh09-6!5zd%dx-l1IQaVwG8|&ubaDscHyvCUn zygu;tYYLQsu$9j6NkFr0%3Vu( zQyEUga9Mh|?f_4Y;X7uiHq}GsXOx*(1+S)Nu8b9*-3+~bfDv|C?4Z!qPzGc# z%rh%F)G#yO-<**}-Xytmj$dC%?3&!|6zSdWDs)$q+>xJ&#Ln}_N{-C|SNg4s=a}?0 z1HgQqF@_z?1j}iH9mRP40Py07geN6BOr7^as@vafW}iuui@h!VY}oB?(4)T4vGaAd zU5|Q^mb92Or!AZ?r9&i5qTXhOzwBCP<~y=$>I!PNW26@78B(;da_}M7NfFoUdU8Hc`Q0KVm1r&4hm$^0!Q=8o-CdSGaL`RdRdDy}k zd=yj|P-2%i5haNH(f>wo=a#fwURvVbw8<AD5@K3(5x!k^Oht#zIK z$LYG=ufOEimCgJlU9W`#Y6tj(bbS=XCm+dc-ME_owkiNL3Z7i)jT{-^(A5;V3Fpo? z*V6+Cb4J1CH5o?@@DhDtGUCg-kt^w%K9@b1Fn9f$A2DI2k88&2x^Bj>b{12QP=+7+)+@OQzU2!27`rJ?R$Pf` zs9P>$0*A0=t*sa`M&xKQp)h46&TkspY}dZ}bpkuU_{3nbl{>xmI8ifFDhz6dAGZ|L z+2>! + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:669371.0 %
Date:2024-04-09 22:03:33Functions: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..4b8ce6f53f --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:669371.0 %
Date:2024-04-09 22:03:33Functions: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..35a0d1c220 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:669371.0 %
Date:2024-04-09 22:03:33Functions: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..b28ede156f --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:669371.0 %
Date:2024-04-09 22:03:33Functions: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..a0a0d63975 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:669371.0 %
Date:2024-04-09 22:03:33Functions: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..73c417fb82 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:669371.0 %
Date:2024-04-09 22:03:33Functions: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..71d9b58e57 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/misc.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/misc.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - misc.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-04-09 22:03:33Functions: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&)102114
+
+
+ + + +
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..ee793eb533 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/misc.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/misc.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - misc.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-04-09 22:03:33Functions: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&)102114
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..3910c03691 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.html @@ -0,0 +1,408 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/misc.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/geometry - misc.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-04-09 22:03:33Functions: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      102120 :     double headingFromRot(const T& rot)
+      58             :     {
+      59      102120 :       const vec3_t rot_vec = rot*vec3_t::UnitX();
+      60      204240 :       return std::atan2(rot_vec.y(), rot_vec.x());
+      61             :     }
+      62             : 
+      63             :     //}
+      64             : 
+      65             :     /* angleBetween() //{ */
+      66             : 
+      67             :     /*!
+      68             :      * \brief Returns the angle between two vectors, taking orientation into account.
+      69             :      *
+      70             :      * This implementation uses \p atan2 instead of just \p acos and thus it properly
+      71             :      * takes into account orientation of the vectors, returning angle in all four quadrants.
+      72             :      *
+      73             :      * \param a vector from which the angle will be measured.
+      74             :      * \param b vector to which the angle will be measured.
+      75             :      *
+      76             :      * \returns    angle from \p a to \p b.
+      77             :      *
+      78             :      */
+      79             :     double angleBetween(const vec2_t& a, const vec2_t& b);
+      80             : 
+      81             :     /*!
+      82             :      * \brief Returns the angle between two vectors, taking orientation into account.
+      83             :      *
+      84             :      * This implementation uses \p atan2 instead of just \p acos and thus it properly
+      85             :      * takes into account orientation of the vectors, returning angle in all four quadrants.
+      86             :      *
+      87             :      * \param a vector from which the angle will be measured.
+      88             :      * \param b vector to which the angle will be measured.
+      89             :      *
+      90             :      * \returns    angle from \p a to \p b.
+      91             :      *
+      92             :      */
+      93             :     double angleBetween(const vec3_t& a, const vec3_t& b);
+      94             : 
+      95             :     //}
+      96             : 
+      97             :     /* angleaxisBetween() //{ */
+      98             : 
+      99             :     /*!
+     100             :      * \brief Returns the rotation between two vectors, represented as angle-axis.
+     101             :      *
+     102             :      * To avoid singularities, a \p tolerance parameter is used:
+     103             :      * * If the absolute angle between the two vectors is less than \p tolerance, a zero rotation is returned.
+     104             :      * * If the angle between the two vectors is closer to \f$ \pi \f$ than \p tolerance, a \f$ \pi \f$ rotation is returned.
+     105             :      *
+     106             :      * \param a vector from which the rotation starts.
+     107             :      * \param b vector at which the rotation ends.
+     108             :      *
+     109             :      * \returns    rotation from \p a to \p b.
+     110             :      *
+     111             :      */
+     112             :     anax_t angleaxisBetween(const vec3_t& a, const vec3_t& b, const double tolerance = 1e-9);
+     113             : 
+     114             :     //}
+     115             : 
+     116             :     /* quaternionBetween() //{ */
+     117             : 
+     118             :     /*!
+     119             :      * \brief Returns the rotation between two vectors, represented as a quaternion.
+     120             :      *
+     121             :      * Works the same as the angleaxisBetween() function (in fact it is used in the implementation).
+     122             :      *
+     123             :      * \param a vector from which the rotation starts.
+     124             :      * \param b vector at which the rotation ends.
+     125             :      *
+     126             :      * \returns    rotation from \p a to \p b.
+     127             :      *
+     128             :      */
+     129             :     quat_t quaternionBetween(const vec3_t& a, const vec3_t& b, const double tolerance = 1e-9);
+     130             : 
+     131             :     //}
+     132             : 
+     133             :     /* rotationBetween() //{ */
+     134             : 
+     135             :     /*!
+     136             :      * \brief Returns the rotation between two vectors, represented as a rotation matrix.
+     137             :      *
+     138             :      * Works the same as the angleaxisBetween() function (in fact it is used in the implementation).
+     139             :      *
+     140             :      * \param a vector from which the rotation starts.
+     141             :      * \param b vector at which the rotation ends.
+     142             :      *
+     143             :      * \returns    rotation from \p a to \p b.
+     144             :      *
+     145             :      */
+     146             :     Eigen::Matrix3d rotationBetween(const vec3_t& a, const vec3_t& b, const double tolerance = 1e-9);
+     147             : 
+     148             :     //}
+     149             : 
+     150             :     /* haversin() //{ */
+     151             : 
+     152             :     /**
+     153             :      * @brief computes the haversine (half of versine) for a given angle
+     154             :      *
+     155             :      * @param angle angle in radians
+     156             :      *
+     157             :      * @return
+     158             :      */
+     159             :     double haversin(const double angle);
+     160             : 
+     161             :     //}
+     162             : 
+     163             :     /* invHaversin() //{ */
+     164             : 
+     165             :     /**
+     166             :      * @brief computes the inverse haversine angle for a given value
+     167             :      *
+     168             :      * @param value
+     169             :      *
+     170             :      * @return angle in radians
+     171             :      */
+     172             :     double invHaversin(const double value);
+     173             : 
+     174             :     //}
+     175             : 
+     176             :     /* solidAngle //{ */
+     177             : 
+     178             :     /**
+     179             :      * @brief computes the solid angle for a spherical surface corresponding to a 'triangle' with edge lengths a, b, c
+     180             :      *
+     181             :      * @param a
+     182             :      * @param b
+     183             :      * @param c
+     184             :      *
+     185             :      * @return solid angle in steradians
+     186             :      */
+     187             :     double solidAngle(double a, double b, double c);
+     188             : 
+     189             :     //}
+     190             : 
+     191             :     /* quaternionFromEuler() //{ */
+     192             : 
+     193             :     /**
+     194             :      * @brief create a quaternion from 3 provided Euler angles
+     195             :      *
+     196             :      * @param x Euler angle in radians
+     197             :      * @param y Euler angle in radians
+     198             :      * @param z Euler angle in radians
+     199             :      *
+     200             :      * @return quaternion
+     201             :      */
+     202             :     quat_t quaternionFromEuler(double x, double y, double z);
+     203             : 
+     204             :     /**
+     205             :      * @brief create a quaternion from Euler angles provided as a vector
+     206             :      *
+     207             :      * @param euler components of the rotation provided as vector of Euler angles
+     208             :      *
+     209             :      * @return quaternion
+     210             :      */
+     211             :     quat_t quaternionFromEuler(const Eigen::Vector3d& euler);
+     212             : 
+     213             :     //}
+     214             : 
+     215             :     /**
+     216             :      * @brief create a quaternion from heading
+     217             :      *
+     218             :      * Heading is defined as the angle of a [1,0,0] vector rotated by the rotation and projected to the XY plane from the X axis.
+     219             :      *
+     220             :      * @param heading the heading angle.
+     221             :      *
+     222             :      * @return the quaternion corresponding to the heading rotation.
+     223             :      */
+     224             :     quat_t quaternionFromHeading(const double heading);
+     225             : 
+     226             :     //}
+     227             : 
+     228             :     // | ----------------- Miscellaneous functions ---------------- |
+     229             : 
+     230             :     /* 2D cross() //{ */
+     231             : 
+     232             :     /*!
+     233             :      * \brief Implementation of cross product for 2D vectors.
+     234             :      *
+     235             :      * Useful e.g. for finding the sine of an angle between two 2D vectors.
+     236             :      *
+     237             :      * \param a first vector of the cross product.
+     238             :      * \param b second vector of the cross product.
+     239             :      *
+     240             :      * \returns    \f$ a \times b \f$ (sine of the angle from \p a to \p b).
+     241             :      *
+     242             :      */
+     243             :     double cross(const vec2_t& a, const vec2_t b);
+     244             : 
+     245             :     //}
+     246             : 
+     247             :     /* vector distance //{ */
+     248             : 
+     249             :     /**
+     250             :      * @brief distnace between two 2D Eigen vectors
+     251             :      *
+     252             :      * @param a
+     253             :      * @param b
+     254             :      *
+     255             :      * @return Euclidean distance
+     256             :      */
+     257             :     double dist(const vec2_t& a, const vec2_t& b);
+     258             : 
+     259             :     /**
+     260             :      * @brief distnace between two 3D Eigen vectors
+     261             :      *
+     262             :      * @param a
+     263             :      * @param b
+     264             :      *
+     265             :      * @return Euclidean distance
+     266             :      */
+     267             :     double dist(const vec3_t& a, const vec3_t& b);
+     268             : 
+     269             :     //}
+     270             : 
+     271             :     /* triangleArea() //{ */
+     272             : 
+     273             :     /**
+     274             :      * @brief uses Heron's formula to compute area of a given triangle using side lengths
+     275             :      *
+     276             :      * @param a length of side1
+     277             :      * @param b length of side2
+     278             :      * @param c length of side3
+     279             :      *
+     280             :      * @return area in units squared
+     281             :      */
+     282             :     double triangleArea(const double a, const double b, const double c);
+     283             : 
+     284             :     //}
+     285             : 
+     286             :     /* sphericalTriangleArea //{ */
+     287             : 
+     288             :     /**
+     289             :      * @brief compute the area of a 'triangle' drawn on a spherical surface
+     290             :      *
+     291             :      * @param a length of side1
+     292             :      * @param b length of side2
+     293             :      * @param c length of side3
+     294             :      *
+     295             :      * @return area in units squared
+     296             :      */
+     297             :     double sphericalTriangleArea(Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d c);
+     298             : 
+     299             :     //}
+     300             : 
+     301             :     /* rotateCovariance() //{ */
+     302             :     
+     303             :     /*!
+     304             :      * \brief Returns the covariance rotated using the specified rotation.
+     305             :      *
+     306             :      * \param cov the covariance to be rotated.
+     307             :      * \param rot the rotation use.
+     308             :      *
+     309             :      * \returns a new matrix object containing the rotated covariance.
+     310             :      *
+     311             :      */
+     312             :     template <typename T>
+     313             :     mat3_t rotateCovariance(const mat3_t& cov, const T& rot)
+     314             :     {
+     315             :       const mat3_t matrot(rot);
+     316             :       return matrot*cov*matrot.transpose();
+     317             :     }
+     318             :     
+     319             :     //}
+     320             : 
+     321             :   }  // namespace geometry
+     322             : }  // namespace mrs_lib
+     323             : 
+     324             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.overview.html b/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.overview.html new file mode 100644 index 0000000000..5f250b8ef8 --- /dev/null +++ b/mrs_lib/include/mrs_lib/geometry/misc.h.gcov.overview.html @@ -0,0 +1,101 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/geometry/misc.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::UTMLetterDesignator(double)171497
mrs_lib::UTM(double, double, double*, double*)8221
mrs_lib::LLtoUTM(double, double, double&, double&, char*)171521
mrs_lib::UTMtoLL(double, double, char const*, double&, double&)1793
+
+
+ + + +
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..31ae535fa5 --- /dev/null +++ b/mrs_lib/include/mrs_lib/gps_conversions.h.gcov.html @@ -0,0 +1,387 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/gps_conversions.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - gps_conversions.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10413676.5 %
Date:2024-04-09 22:03:33Functions: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        8221 :   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        8221 :     int cm = ((lon >= 0.0) ? ((int)lon - ((int)lon) % 6 + 3) : ((int)lon - ((int)lon) % 6 - 3));
+      63             : 
+      64             :     // convert degrees into radians
+      65        8221 :     double rlat  = lat * RADIANS_PER_DEGREE;
+      66        8221 :     double rlon  = lon * RADIANS_PER_DEGREE;
+      67        8221 :     double rlon0 = cm * RADIANS_PER_DEGREE;
+      68             : 
+      69             :     // compute trigonometric functions
+      70        8221 :     double slat = sin(rlat);
+      71        8221 :     double clat = cos(rlat);
+      72        8221 :     double tlat = tan(rlat);
+      73             : 
+      74             :     // decide the false northing at origin
+      75        8221 :     double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;
+      76             : 
+      77        8221 :     double T = tlat * tlat;
+      78        8221 :     double C = UTM_EP2 * clat * clat;
+      79        8221 :     double A = (rlon - rlon0) * clat;
+      80        8221 :     double M = WGS84_A * (m0 * rlat + m1 * sin(2 * rlat) + m2 * sin(4 * rlat) + m3 * sin(6 * rlat));
+      81        8221 :     double V = WGS84_A / sqrt(1 - UTM_E2 * slat * slat);
+      82             : 
+      83             :     // compute the easting-northing coordinates
+      84        8221 :     *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        8153 :     *y = fn +
+      86        8153 :          UTM_K0 *
+      87        8192 :              (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        8153 :     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      171497 :   static inline char UTMLetterDesignator(double Lat) {
+     102             :     char LetterDesignator;
+     103             : 
+     104      171497 :     if ((84 >= Lat) && (Lat >= 72))
+     105           0 :       LetterDesignator = 'X';
+     106      171497 :     else if ((72 > Lat) && (Lat >= 64))
+     107           0 :       LetterDesignator = 'W';
+     108      171497 :     else if ((64 > Lat) && (Lat >= 56))
+     109           0 :       LetterDesignator = 'V';
+     110      171497 :     else if ((56 > Lat) && (Lat >= 48))
+     111           0 :       LetterDesignator = 'U';
+     112      171497 :     else if ((48 > Lat) && (Lat >= 40))
+     113      170370 :       LetterDesignator = 'T';
+     114        1127 :     else if ((40 > Lat) && (Lat >= 32))
+     115           0 :       LetterDesignator = 'S';
+     116        1127 :     else if ((32 > Lat) && (Lat >= 24))
+     117           0 :       LetterDesignator = 'R';
+     118        1127 :     else if ((24 > Lat) && (Lat >= 16))
+     119           0 :       LetterDesignator = 'Q';
+     120        1127 :     else if ((16 > Lat) && (Lat >= 8))
+     121           0 :       LetterDesignator = 'P';
+     122        1127 :     else if ((8 > Lat) && (Lat >= 0))
+     123           2 :       LetterDesignator = 'N';
+     124        1125 :     else if ((0 > Lat) && (Lat >= -8))
+     125           0 :       LetterDesignator = 'M';
+     126        1125 :     else if ((-8 > Lat) && (Lat >= -16))
+     127           0 :       LetterDesignator = 'L';
+     128        1125 :     else if ((-16 > Lat) && (Lat >= -24))
+     129           0 :       LetterDesignator = 'K';
+     130        1125 :     else if ((-24 > Lat) && (Lat >= -32))
+     131           0 :       LetterDesignator = 'J';
+     132        1125 :     else if ((-32 > Lat) && (Lat >= -40))
+     133           0 :       LetterDesignator = 'H';
+     134        1125 :     else if ((-40 > Lat) && (Lat >= -48))
+     135           0 :       LetterDesignator = 'G';
+     136        1125 :     else if ((-48 > Lat) && (Lat >= -56))
+     137           0 :       LetterDesignator = 'F';
+     138        1125 :     else if ((-56 > Lat) && (Lat >= -64))
+     139           0 :       LetterDesignator = 'E';
+     140        1125 :     else if ((-64 > Lat) && (Lat >= -72))
+     141           0 :       LetterDesignator = 'D';
+     142        1125 :     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        1125 :       LetterDesignator = 'Z';
+     147      171497 :     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      171521 :   static inline void LLtoUTM(const double Lat, const double Long, double& UTMNorthing, double& UTMEasting, char* UTMZone) {
+     160      171521 :     double a          = WGS84_A;
+     161      171521 :     double eccSquared = UTM_E2;
+     162      171521 :     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      171521 :     double LongTemp = (Long + 180) - int((Long + 180) / 360) * 360 - 180;
+     170             : 
+     171      171521 :     double LatRad  = Lat * RADIANS_PER_DEGREE;
+     172      171521 :     double LongRad = LongTemp * RADIANS_PER_DEGREE;
+     173             :     double LongOriginRad;
+     174             :     int    ZoneNumber;
+     175             : 
+     176      171521 :     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      171521 :     if (ZoneNumber > 99)
+     180           0 :       ZoneNumber = 99;
+     181      171521 :     if (ZoneNumber < -9)
+     182           0 :       ZoneNumber = -9;
+     183             : 
+     184      171521 :     if (Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0)
+     185           0 :       ZoneNumber = 32;
+     186             : 
+     187             :     // Special zones for Svalbard
+     188      171521 :     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      171521 :     LongOrigin    = (ZoneNumber - 1) * 6 - 180 + 3;
+     200      171521 :     LongOriginRad = LongOrigin * RADIANS_PER_DEGREE;
+     201             : 
+     202             :     // compute the UTM Zone from the latitude and longitude
+     203      171521 :     snprintf(UTMZone, 4, "%d%c", ZoneNumber, UTMLetterDesignator(Lat));
+     204             : 
+     205      170834 :     eccPrimeSquared = (eccSquared) / (1 - eccSquared);
+     206             : 
+     207      170834 :     N = a / sqrt(1 - eccSquared * sin(LatRad) * sin(LatRad));
+     208      170834 :     T = tan(LatRad) * tan(LatRad);
+     209      170834 :     C = eccPrimeSquared * cos(LatRad) * cos(LatRad);
+     210      170834 :     A = cos(LatRad) * (LongRad - LongOriginRad);
+     211             : 
+     212      170834 :     M = a * ((1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256) * LatRad -
+     213      170834 :              (3 * eccSquared / 8 + 3 * eccSquared * eccSquared / 32 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(2 * LatRad) +
+     214      170834 :              (15 * eccSquared * eccSquared / 256 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(4 * LatRad) -
+     215      170834 :              (35 * eccSquared * eccSquared * eccSquared / 3072) * sin(6 * LatRad));
+     216             : 
+     217      170834 :     UTMEasting =
+     218      170834 :         (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      170834 :     UTMNorthing = (double)(k0 * (M + N * tan(LatRad) *
+     221      170834 :                                          (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 +
+     222      170834 :                                           (61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A * A * A * A * A * A / 720)));
+     223      170834 :     if (Lat < 0)
+     224           0 :       UTMNorthing += 10000000.0;  // 10000000 meter offset for southern hemisphere
+     225      170834 :   }
+     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        1793 :   static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting, const char* UTMZone, double& Lat, double& Long) {
+     246        1793 :     double                  k0         = UTM_K0;
+     247        1793 :     double                  a          = WGS84_A;
+     248        1793 :     double                  eccSquared = UTM_E2;
+     249             :     double                  eccPrimeSquared;
+     250        1793 :     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        1793 :     x = UTMEasting - 500000.0;  // remove 500,000 meter offset for longitude
+     261        1793 :     y = UTMNorthing;
+     262             : 
+     263        1793 :     ZoneNumber = strtoul(UTMZone, &ZoneLetter, 10);
+     264        1793 :     if ((*ZoneLetter - 'N') >= 0)
+     265        1793 :       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        1793 :     LongOrigin = (ZoneNumber - 1) * 6 - 180 + 3;  //+3 puts origin in middle of zone
+     272             : 
+     273        1793 :     eccPrimeSquared = (eccSquared) / (1 - eccSquared);
+     274             : 
+     275        1793 :     M  = y / k0;
+     276        1793 :     mu = M / (a * (1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256));
+     277             : 
+     278        1793 :     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        1793 :               (151 * e1 * e1 * e1 / 96) * sin(6 * mu);
+     280        1793 :     phi1 = phi1Rad * DEGREES_PER_RADIAN;
+     281             : 
+     282        1793 :     N1 = a / sqrt(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad));
+     283        1793 :     T1 = tan(phi1Rad) * tan(phi1Rad);
+     284        1793 :     C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad);
+     285        1793 :     R1 = a * (1 - eccSquared) / pow(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad), 1.5);
+     286        1793 :     D  = x / (N1 * k0);
+     287             : 
+     288        1793 :     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        1793 :                                                 (61 + 90 * T1 + 298 * C1 + 45 * T1 * T1 - 252 * eccPrimeSquared - 3 * C1 * C1) * D * D * D * D * D * D / 720);
+     290        1793 :     Lat = Lat * DEGREES_PER_RADIAN;
+     291             : 
+     292        1793 :     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        1793 :            cos(phi1Rad);
+     294        1793 :     Long = LongOrigin + Long * DEGREES_PER_RADIAN;
+     295        1793 :   }
+     296             : 
+     297             :   static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting, std::string UTMZone, double& Lat, double& Long) {
+     298             :     UTMtoLL(UTMNorthing, UTMEasting, UTMZone.c_str(), Lat, Long);
+     299             :   }
+     300             : 
+     301             : }  // namespace mrs_lib
+     302             : 
+     303             : #endif  // _UTM_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/gps_conversions.h.gcov.overview.html b/mrs_lib/include/mrs_lib/gps_conversions.h.gcov.overview.html new file mode 100644 index 0000000000..68d9ba32f7 --- /dev/null +++ b/mrs_lib/include/mrs_lib/gps_conversions.h.gcov.overview.html @@ -0,0 +1,96 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/gps_conversions.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
subscribe_handler.hpp +
81.7%81.7%
+ 		81.7 %98 / 12046.6 %487 / 1044
<unnamed>81.7 %98 / 12046.6 %487 / 1044
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
<unnamed>74.0 %77 / 10466.7 %8 / 12
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6483.3 %40 / 48
<unnamed>85.9 %55 / 6483.3 %40 / 48
publisher_handler.hpp +
81.5%81.5%
+ 		81.5 %44 / 5497.5 %197 / 202
<unnamed>81.5 %44 / 5497.5 %197 / 202
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
<unnamed>87.5 %14 / 16100.0 %3 / 3
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 14100.0 %16 / 16
<unnamed>64.3 %9 / 14100.0 %16 / 16
vector_converter.hpp +
100.0%
+ 		100.0 %18 / 18100.0 %18 / 18
<unnamed>100.0 %18 / 18100.0 %18 / 18
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %37 / 37
<unnamed>92.9 %52 / 56100.0 %37 / 37
+
+
+ + + + +
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..360542b6b6 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index-detail-sort-l.html @@ -0,0 +1,236 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:36744682.3 %
Date:2024-04-09 22:03:33Functions:806138058.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 14100.0 %16 / 16
<unnamed>64.3 %9 / 14100.0 %16 / 16
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
<unnamed>74.0 %77 / 10466.7 %8 / 12
publisher_handler.hpp +
81.5%81.5%
+ 		81.5 %44 / 5497.5 %197 / 202
<unnamed>81.5 %44 / 5497.5 %197 / 202
subscribe_handler.hpp +
81.7%81.7%
+ 		81.7 %98 / 12046.6 %487 / 1044
<unnamed>81.7 %98 / 12046.6 %487 / 1044
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6483.3 %40 / 48
<unnamed>85.9 %55 / 6483.3 %40 / 48
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
<unnamed>87.5 %14 / 16100.0 %3 / 3
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %37 / 37
<unnamed>92.9 %52 / 56100.0 %37 / 37
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..20e0871807 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index-detail.html @@ -0,0 +1,236 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:36744682.3 %
Date:2024-04-09 22:03:33Functions:806138058.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 14100.0 %16 / 16
<unnamed>64.3 %9 / 14100.0 %16 / 16
publisher_handler.hpp +
81.5%81.5%
+ 		81.5 %44 / 5497.5 %197 / 202
<unnamed>81.5 %44 / 5497.5 %197 / 202
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %37 / 37
<unnamed>92.9 %52 / 56100.0 %37 / 37
subscribe_handler.hpp +
81.7%81.7%
+ 		81.7 %98 / 12046.6 %487 / 1044
<unnamed>81.7 %98 / 12046.6 %487 / 1044
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
<unnamed>87.5 %14 / 16100.0 %3 / 3
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6483.3 %40 / 48
<unnamed>85.9 %55 / 6483.3 %40 / 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..cca7fd8023 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index-sort-f.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:36744682.3 %
Date:2024-04-09 22:03:33Functions:806138058.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
subscribe_handler.hpp +
81.7%81.7%
+ 		81.7 %98 / 12046.6 %487 / 1044
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6483.3 %40 / 48
publisher_handler.hpp +
81.5%81.5%
+ 		81.5 %44 / 5497.5 %197 / 202
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 14100.0 %16 / 16
vector_converter.hpp +
100.0%
+ 		100.0 %18 / 18100.0 %18 / 18
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %37 / 37
+
+
+ + + + +
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..05d62d9766 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index-sort-l.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:36744682.3 %
Date:2024-04-09 22:03:33Functions:806138058.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 14100.0 %16 / 16
ukf.hpp +
74.0%74.0%
+ 		74.0 %77 / 10466.7 %8 / 12
publisher_handler.hpp +
81.5%81.5%
+ 		81.5 %44 / 5497.5 %197 / 202
subscribe_handler.hpp +
81.7%81.7%
+ 		81.7 %98 / 12046.6 %487 / 1044
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6483.3 %40 / 48
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %37 / 37
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..3a494839b1 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/index.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/implHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:36744682.3 %
Date:2024-04-09 22:03:33Functions:806138058.4 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
param_provider.hpp +
64.3%64.3%
+ 		64.3 %9 / 14100.0 %16 / 16
publisher_handler.hpp +
81.5%81.5%
+ 		81.5 %44 / 5497.5 %197 / 202
service_client_handler.hpp +
92.9%92.9%
+ 		92.9 %52 / 56100.0 %37 / 37
subscribe_handler.hpp +
81.7%81.7%
+ 		81.7 %98 / 12046.6 %487 / 1044
timer.hpp +
87.5%87.5%
+ 		87.5 %14 / 16100.0 %3 / 3
transformer.hpp +
85.9%85.9%
+ 		85.9 %55 / 6483.3 %40 / 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..0c4aa32785 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.func-sort-c.html @@ -0,0 +1,144 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/param_provider.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - param_provider.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:91464.3 %
Date:2024-04-09 22:03:33Functions: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> >&) const1467
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> >&) const1467
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> > > >&) const2862
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> > > >&) const2862
bool mrs_lib::ParamProvider::getParamImpl<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const4148
bool mrs_lib::ParamProvider::getParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const4148
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> >&) const13827
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> >&) const13827
bool mrs_lib::ParamProvider::getParamImpl<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const16432
bool mrs_lib::ParamProvider::getParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&) const16432
bool mrs_lib::ParamProvider::getParamImpl<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const33483
bool mrs_lib::ParamProvider::getParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const33483
+
+
+ + + +
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..da0e701795 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.func.html @@ -0,0 +1,144 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/param_provider.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - param_provider.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:91464.3 %
Date:2024-04-09 22:03:33Functions: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> >&) const13827
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> > > >&) const2862
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> >&) const1467
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&) const16432
bool mrs_lib::ParamProvider::getParamImpl<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const33483
bool mrs_lib::ParamProvider::getParamImpl<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const4148
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> >&) const13827
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> > > >&) const2862
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> >&) const1467
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&) const16432
bool mrs_lib::ParamProvider::getParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&) const33483
bool mrs_lib::ParamProvider::getParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&) const4148
+
+
+ + + +
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..547257977b --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/param_provider.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - param_provider.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:91464.3 %
Date:2024-04-09 22:03:33Functions: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       72234 :   bool ParamProvider::getParam(const std::string& param_name, T& value_out) const
+      10             :   {
+      11             :     try
+      12             :     {
+      13       72234 :       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       72234 :   bool ParamProvider::getParamImpl(const std::string& param_name, T& value_out) const
+      24             :   {
+      25             :     {
+      26       72234 :       const auto found_node = findYamlNode(param_name);
+      27       72234 :       if (found_node.has_value())
+      28             :       {
+      29             :         try
+      30             :         {
+      31             :           // try catch is the only type-generic option...
+      32       62115 :           value_out = found_node.value().as<T>();
+      33       62115 :           return true;
+      34             :         }
+      35           0 :         catch (const YAML::BadConversion& e)
+      36             :         {}
+      37             :       }
+      38             : 
+      39             :     }
+      40             : 
+      41       10119 :     if (m_use_rosparam)
+      42       10119 :       return m_nh.getParam(param_name, value_out);
+      43             : 
+      44           0 :     return false;
+      45             :   }
+      46             : }
+      47             : 
+      48             : #endif // PARAM_PROVIDER_HPP
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.overview.html b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.overview.html new file mode 100644 index 0000000000..69615a5af4 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/param_provider.hpp.gcov.overview.html @@ -0,0 +1,32 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/param_provider.hpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)0
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > > const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > const&)0
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)0
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)1
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > > const&)1
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::publish(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::PublisherHandler_impl<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::PublisherHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)1
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::publish(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::PublisherHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned int const&, bool const&, double const&)2
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > > const&)2
mrs_lib::PublisherHandler<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<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&)25
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > > const&)25
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&)25
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&)82
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > > const&)82
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&)82
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > > const&)82
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&)82
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > > const&)82
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&)82
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)82
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&)82
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > > const&)82
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&)82
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&)82
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&)82
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&)82
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > > const&)82
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&)82
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&)82
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&)82
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > > const&)82
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&)82
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > > const&)82
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&)82
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > > const&)82
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&)82
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > > const&)82
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&)82
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > > const&)82
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&)82
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&)82
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&)82
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > > const&)82
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&)82
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&)82
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > > const&)82
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&)82
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > > const&)82
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&)82
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > > const&)82
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&)82
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > > const&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)82
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&)83
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&)83
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&)85
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > > const&)85
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&)85
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::publish(std_msgs::Int64_<std::allocator<void> > const&)110
mrs_lib::PublisherHandler_impl<std_msgs::Int64_<std::allocator<void> > >::publish(std_msgs::Int64_<std::allocator<void> > const&)110
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > > const&)164
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > > const&)164
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > > const&)164
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > > const&)164
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > > const&)164
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > > const&)164
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > > const&)164
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > > const&)164
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > > const&)165
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&)168
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > > const&)168
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&)168
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&)231
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > > const&)231
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&)231
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&)316
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > > const&)316
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&)316
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&)328
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > > const&)328
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&)328
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > > const&)328
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&)328
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&)328
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)333
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&)399
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&)399
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::publish(mrs_msgs::ObstacleSectors_<std::allocator<void> > const&)439
mrs_lib::PublisherHandler_impl<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::publish(mrs_msgs::ObstacleSectors_<std::allocator<void> > const&)439
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&)471
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > > const&)471
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&)471
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)477
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)477
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&)492
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > > const&)492
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&)492
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&)608
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > > const&)608
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&)608
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)997
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)997
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1042
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1042
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)1329
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)1329
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)1509
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)1509
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)1510
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)1510
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)1791
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::publish(mrs_msgs::FutureTrajectory_<std::allocator<void> > const&)1791
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)1832
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)1832
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2001
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2001
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2253
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2253
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3816
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3816
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3845
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3845
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)6542
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)6542
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)7897
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)7897
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)9171
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)9171
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)9516
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)9516
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)10221
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)10221
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)12840
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)12840
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)15124
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)15124
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)15426
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)15426
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)19137
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::publish(std_msgs::String_<std::allocator<void> > const&)19137
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)19137
mrs_lib::PublisherHandler_impl<std_msgs::String_<std::allocator<void> > >::publish(std_msgs::String_<std::allocator<void> > const&)19137
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)28633
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)28633
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)72756
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)72756
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)78813
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)78813
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)86602
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)86602
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)121312
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)121312
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)131055
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)131055
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)132935
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)173386
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)173454
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)198490
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)198490
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)306408
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)306417
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)313500
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)313585
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)421418
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)421851
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)596061
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)596145
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)671762
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)671784
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)1204047
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)1204187
+
+
+ + + +
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..74b6cc232d --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.func.html @@ -0,0 +1,888 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/publisher_handler.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - publisher_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445481.5 %
Date:2024-04-09 22:03:33Functions:19720297.5 %
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&)131055
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&)328
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > > const&)328
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)1329
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&)82
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > > const&)82
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)173454
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&)85
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > > const&)85
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)28633
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&)492
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > > const&)492
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)9171
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&)82
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > > const&)82
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)72756
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&)82
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > > const&)82
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)306417
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&)471
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > > const&)471
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)86602
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&)82
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)82
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)596061
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&)316
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > > const&)316
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)6542
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&)439
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&)1791
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&)82
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > > const&)82
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3816
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&)82
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > > const&)164
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)7897
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&)82
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > > const&)164
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)477
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&)82
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > > const&)164
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)12840
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&)82
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > > const&)82
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)1204187
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&)608
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > > const&)608
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)421851
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&)231
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > > const&)231
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2001
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&)83
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > > const&)165
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&)78813
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&)82
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > > const&)164
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3845
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&)82
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > > const&)164
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)121312
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&)82
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > > const&)82
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)15426
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&)82
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > > const&)82
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)19137
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&)82
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > > const&)82
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)1832
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&)82
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > > const&)82
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)1509
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&)82
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > > const&)82
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1042
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&)82
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > > const&)164
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2253
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&)82
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > > const&)164
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)15124
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&)82
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > > const&)82
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)997
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&)82
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > > const&)164
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)1510
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&)82
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > > const&)82
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&)82
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > > const&)82
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)313585
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&)168
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > > const&)168
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)671784
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)132935
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&)399
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > > const&)333
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)9516
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&)25
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > > const&)25
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)10221
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&)82
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > > const&)82
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&)19137
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&)82
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > > const&)82
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)198490
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&)328
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > > const&)328
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::publish(geometry_msgs::PoseArray_<std::allocator<void> > const&)131055
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&)328
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::publish(geometry_msgs::PoseStamped_<std::allocator<void> > const&)1329
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&)82
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::publish(geometry_msgs::QuaternionStamped_<std::allocator<void> > const&)173386
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&)85
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::publish(visualization_msgs::MarkerArray_<std::allocator<void> > const&)28633
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&)492
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::publish(mrs_msgs::ControlError_<std::allocator<void> > const&)9171
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorInput_<std::allocator<void> > const&)72756
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::publish(mrs_msgs::Float64Stamped_<std::allocator<void> > const&)306408
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&)471
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::publish(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)86602
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::publish(mrs_msgs::EstimatorOutput_<std::allocator<void> > const&)596145
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&)316
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::publish(mrs_msgs::HwApiRcChannels_<std::allocator<void> > const&)6542
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&)439
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&)1791
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3816
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)7897
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)477
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::publish(mrs_msgs::DynamicsConstraints_<std::allocator<void> > const&)12840
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::publish(mrs_msgs::EstimatorCorrection_<std::allocator<void> > const&)1204047
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&)608
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::publish(mrs_msgs::Float64ArrayStamped_<std::allocator<void> > const&)421418
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&)231
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)2001
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&)83
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&)78813
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3845
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const&)121312
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const&)15426
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const&)19137
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const&)1832
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const&)1509
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1042
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2253
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const&)15124
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)997
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::publish(mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const&)1510
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&)82
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&)82
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::publish(mrs_msgs::UavState_<std::allocator<void> > const&)313500
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&)168
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::publish(nav_msgs::Odometry_<std::allocator<void> > const&)671762
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&)399
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::publish(std_msgs::Bool_<std::allocator<void> > const&)9516
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&)25
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::publish(std_msgs::Empty_<std::allocator<void> > const&)10221
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&)82
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&)19137
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&)82
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::publish(std_msgs::Float64_<std::allocator<void> > const&)198490
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&)328
+
+
+ + + +
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..46da6f6e6a --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/publisher_handler.hpp.gcov.html @@ -0,0 +1,332 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/publisher_handler.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - publisher_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445481.5 %
Date:2024-04-09 22:03:33Functions:19720297.5 %
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        5507 : PublisherHandler_impl<TopicType>::PublisherHandler_impl(ros::NodeHandle& nh, const std::string& address, const unsigned int& buffer_size, const bool& latch,
+      23        5507 :                                                         const double& rate) {
+      24             : 
+      25             :   {
+      26        5507 :     std::scoped_lock lock(mutex_publisher_);
+      27             : 
+      28        5507 :     publisher_ = nh.advertise<TopicType>(address, buffer_size, latch);
+      29             : 
+      30        5507 :     if (rate > 0.0) {
+      31             : 
+      32         575 :       throttle_ = true;
+      33             : 
+      34         575 :       throttle_min_dt_ = 1.0 / rate;
+      35             : 
+      36             :     } else {
+      37             : 
+      38        4932 :       throttle_ = false;
+      39             : 
+      40        4932 :       throttle_min_dt_ = 0;
+      41             :     }
+      42             : 
+      43        5507 :     last_time_published_ = ros::Time(0);
+      44             :   }
+      45             : 
+      46        5507 :   publisher_initialized_ = true;
+      47        5507 : }
+      48             : 
+      49             : //}
+      50             : 
+      51             : /* publish(TopicType& msg) //{ */
+      52             : 
+      53             : template <class TopicType>
+      54     4552294 : void PublisherHandler_impl<TopicType>::publish(const TopicType& msg) {
+      55             : 
+      56     4552294 :   if (!publisher_initialized_) {
+      57           0 :     return;
+      58             :   }
+      59             : 
+      60             :   {
+      61     4551577 :     std::scoped_lock lock(mutex_publisher_);
+      62             : 
+      63     4551384 :     if (throttle_) {
+      64             : 
+      65      239538 :       if ((ros::Time::now() - last_time_published_).toSec() < throttle_min_dt_) {
+      66      109713 :         return;
+      67             :       }
+      68             : 
+      69      129825 :       last_time_published_ = ros::Time::now();
+      70             :     }
+      71             : 
+      72             :     try {
+      73     4441671 :       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        6177 : PublisherHandler<TopicType>& PublisherHandler<TopicType>::operator=(const PublisherHandler<TopicType>& other) {
+     169             : 
+     170        6177 :   if (this == &other) {
+     171           0 :     return *this;
+     172             :   }
+     173             : 
+     174        6177 :   if (other.impl_) {
+     175        6177 :     this->impl_ = other.impl_;
+     176             :   }
+     177             : 
+     178        6177 :   return *this;
+     179             : }
+     180             : 
+     181             : //}
+     182             : 
+     183             : /* copy constructor //{ */
+     184             : 
+     185             : template <class TopicType>
+     186      132935 : PublisherHandler<TopicType>::PublisherHandler(const PublisherHandler<TopicType>& other) {
+     187             : 
+     188      132935 :   if (other.impl_) {
+     189      132935 :     this->impl_ = other.impl_;
+     190             :   }
+     191      132935 : }
+     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        5507 : PublisherHandler<TopicType>::PublisherHandler(ros::NodeHandle& nh, const std::string& address, const unsigned int& buffer_size, const bool& latch,
+     199        5507 :                                               const double& rate) {
+     200             : 
+     201        5507 :   impl_ = std::make_shared<PublisherHandler_impl<TopicType>>(nh, address, buffer_size, latch, rate);
+     202        5507 : }
+     203             : 
+     204             : //}
+     205             : 
+     206             : /* publish(const TopicType& msg) //{ */
+     207             : 
+     208             : template <class TopicType>
+     209     4552967 : void PublisherHandler<TopicType>::publish(const TopicType& msg) {
+     210             : 
+     211     4552967 :   impl_->publish(msg);
+     212     4554654 : }
+     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..c005cbd0db --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func-sort-c.html @@ -0,0 +1,228 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/service_client_handler.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - service_client_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:525692.9 %
Date:2024-04-09 22:03:33Functions:3737100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::callAsync(std_srvs::Trigger&, int 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::Vec1>::call(mrs_msgs::Vec1&)19
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)19
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)79
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)79
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)82
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)82
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv> const&)82
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)82
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Vec1> const&)82
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::initialize(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)82
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)82
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)82
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)82
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)83
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> const&)83
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)83
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)164
mrs_lib::ServiceClientHandler<mrs_msgs::String>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::String> const&)164
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)164
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::call(std_srvs::Trigger&)192
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&)192
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::call(std_srvs::SetBool&)280
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::call(std_srvs::SetBool&)280
mrs_lib::ServiceClientHandler<mrs_msgs::String>::call(mrs_msgs::String&)317
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::call(mrs_msgs::String&)317
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)492
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::operator=(mrs_lib::ServiceClientHandler<std_srvs::SetBool> const&)492
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)492
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)912
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::operator=(mrs_lib::ServiceClientHandler<std_srvs::Trigger> const&)912
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)994
+
+
+ + + +
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..9ac8c8e21f --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.func.html @@ -0,0 +1,228 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/service_client_handler.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - service_client_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:525692.9 %
Date:2024-04-09 22:03:33Functions:3737100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)79
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)83
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> const&)83
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)82
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)82
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv> const&)82
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)19
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)82
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::Vec1> const&)82
mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler<mrs_msgs::String>::call(mrs_msgs::String&)317
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)164
mrs_lib::ServiceClientHandler<mrs_msgs::String>::operator=(mrs_lib::ServiceClientHandler<mrs_msgs::String> const&)164
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::call(std_srvs::SetBool&)280
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)492
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::operator=(mrs_lib::ServiceClientHandler<std_srvs::SetBool> const&)492
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::initialize(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)82
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::call(std_srvs::Trigger&)192
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&)912
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::operator=(mrs_lib::ServiceClientHandler<std_srvs::Trigger> const&)912
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::call(mrs_msgs::ReferenceStampedSrv&)79
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)83
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::call(mrs_msgs::DynamicsConstraintsSrv&)82
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)82
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::call(mrs_msgs::Vec1&)19
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)82
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec4>::call(mrs_msgs::Vec4&)1
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::call(mrs_msgs::String&)317
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)164
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::call(std_srvs::SetBool&)280
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::ServiceClientHandler_impl(ros::NodeHandle&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)492
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::call(std_srvs::Trigger&)192
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&)994
+
+
+ + + +
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..f735a285a5 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.html @@ -0,0 +1,403 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/service_client_handler.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - service_client_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:525692.9 %
Date:2024-04-09 22:03:33Functions:3737100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.png b/mrs_lib/include/mrs_lib/impl/service_client_handler.hpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..263b73ad1380cf9f7a8ef86c51aca552078dfaf1 GIT binary patch literal 969 zcmV;)12+7LP)Wqk9Iv^@lP(dY(=QXOzU}^O$n3W+JqlhhKurdbsVPJ$4 zm2sw#fHn(q8wtvc3@&CusskcExa5-;CL|9hAa2i^A<8=Aw(a}=Z{L45n3k=@;Th4i zo+G;AC3x?_TV}BL=oh6ew&M%93cw*aU1|285l zT!NU`Mp*~6s%7#VjJVE+;1pg)`&RYb!*R9eBK&bfdM-)ECo1pbkef4EZ$u;FTpg)P zx3}9BbjHow&pHv-#>@V^FkzWGq7i_@V1BnXBT7cyp+S7ZdWwYd9?^X?Sx?~`3g+AB zi?{EDo+51^>hz*Duv_mvWO+jOvI5%_itJ)oVlEcCI)xaE{P+`cY9&_gK=;7v_ zg@l7f#Cmv|5B_Qn2e&pFhJ#l4pTj{!@4{Ust_Bh7;kDcl6MM*J$V{;&kWhNjcS!m@ zX4Bib=l$iw{G;lCNmYo9Mb=aKsEax3d?wu;Sze#!6tcc6k#V+yRHD*pb5cs;XTwjM zJKXO?L{du0Qb;+o5JyeY3<%(%%UefHGE10sW!5aF0me~nVH0i;^<5TL9l1NA@qtxXjPZ-Hy|8-Suo%AbI@I=D1MDetRyXI_3l`vS}O#^lU5 zmu0@;ASZ`RpG3To0T|}uDQ!LTkw$iqE6sxg*#assQV?bPCT_M`SRU@fW91^^N5(Y^ rzi2W3O>xaxC_|mFwRLjam4SZ&Pzvjg528x=00000NkvXXu0mjf<=M*E literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func-sort-c.html b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func-sort-c.html new file mode 100644 index 0000000000..075d427571 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func-sort-c.html @@ -0,0 +1,4256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - subscribe_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9812081.7 %
Date:2024-04-09 22:03:33Functions:487104446.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::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().20
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::~Impl().20
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)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::process_new_message(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)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::process_new_message(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::hasMsg() 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::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const1
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()4
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::getMsg()4
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const4
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<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<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::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const6
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::hasMsg() const6
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&)8
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::start()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)> const&)9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().29
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::start()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)> const&)9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::~Impl().29
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const9
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::start()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().210
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::start()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl().210
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().210
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::start()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::~Impl().210
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const10
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const15
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::hasMsg() const15
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()25
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&)25
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()25
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().225
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::start()25
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&)25
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::~Impl().225
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()25
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&)25
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()25
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().225
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::start()25
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&)25
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().225
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()25
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&)25
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()25
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().225
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::start()25
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&)25
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl().225
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const25
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const25
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const25
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::getMsg()72
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::getMsg()72
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const72
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const72
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::hasMsg() const72
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::peekMsg() const72
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const78
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const78
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const78
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const78
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&)82
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
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&)82
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const83
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::start()85
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&)85
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()85
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().285
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::start()85
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&)85
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl().285
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const85
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::start()86
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::start()86
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const97
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const108
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::start()153
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&)153
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()153
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2153
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::start()153
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&)153
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl().2153
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const153
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const162
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::lastMsgTime() const162
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::start()164
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&)164
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()164
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2164
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::start()164
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&)164
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl().2164
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()164
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&)164
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()164
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2164
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::start()164
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&)164
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl().2164
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::start()164
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&)164
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()164
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2164
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::start()164
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&)164
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl().2164
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const164
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const164
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const164
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::start()167
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&)167
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()167
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2167
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::start()167
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&)167
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl().2167
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const167
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::start()189
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&)189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::start()189
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&)189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl().2189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::getMsg()229
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::getMsg()229
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const229
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::peekMsg() const229
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::start()246
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&)246
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()246
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2246
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::start()246
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&)246
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl().2246
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const246
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::start()252
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&)252
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()252
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2252
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::start()252
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&)252
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl().2252
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const253
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::start()259
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&)259
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()259
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2259
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::start()259
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&)259
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl().2259
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const259
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::start()271
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&)271
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()271
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2271
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::start()271
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&)271
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl().2271
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)294
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)294
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const301
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::lastMsgTime() const301
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::start()313
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&)313
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()313
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2313
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::start()313
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&)313
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl().2313
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const313
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::start()324
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&)324
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()324
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2324
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::start()324
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&)324
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl().2324
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const324
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::start()328
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&)328
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()328
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2328
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::start()328
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&)328
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl().2328
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const328
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()353
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&)353
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()353
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2353
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::start()353
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&)353
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl().2353
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()353
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&)353
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()353
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2353
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::start()353
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&)353
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2353
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const353
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const353
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)390
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)390
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::getMsg()421
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::getMsg()421
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const421
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::peekMsg() const421
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const450
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::start()486
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&)486
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()486
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2486
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::start()486
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&)486
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl().2486
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)532
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > const> const&)532
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::getMsg()549
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::getMsg()549
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const549
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const549
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::hasMsg() const549
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::peekMsg() const549
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::getMsg()611
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::getMsg()611
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const611
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::peekMsg() const611
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const661
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)1509
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)1509
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)1510
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)1510
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)1951
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)1951
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const2357
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::hasMsg() const2357
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)3112
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)3112
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()3808
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::getMsg()3808
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const3808
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::peekMsg() const3808
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)4591
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)4597
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::getMsg()9517
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::getMsg()9517
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const9517
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const9517
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::hasMsg() const9517
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::peekMsg() const9517
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)11531
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)11558
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)12826
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)12826
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const13498
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::getMsg()13503
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::getMsg()13503
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::peekMsg() const13510
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const16411
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::hasMsg() const16411
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::hasMsg() const16970
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const16973
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const17005
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const17005
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const17183
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::hasMsg() const17183
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const17213
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::hasMsg() const17213
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)18280
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)18280
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::getMsg()19280
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::getMsg()19280
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const19280
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::peekMsg() const19280
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)19866
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)19866
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::getMsg()32190
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::getMsg()32190
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const32190
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const32190
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::lastMsgTime() const32190
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::peekMsg() const32190
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()38330
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::getMsg()38345
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const38345
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::peekMsg() const38345
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const46336
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::hasMsg() const46336
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)52018
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)52018
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const59462
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::hasMsg() const59471
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()61970
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const61986
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const61989
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::getMsg()61990
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const63699
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const63729
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)65184
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)65354
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)65418
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)65583
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)121249
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)121249
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const133860
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const133877
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()135106
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const135108
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const135112
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()135113
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::getMsg()151728
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::getMsg()151728
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const151728
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const151728
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::hasMsg() const151728
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::peekMsg() const151728
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const166873
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::getMsg()166896
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)166897
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)166923
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()166942
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::peekMsg() const166965
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)171703
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)171710
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)172861
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)172906
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const179269
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::lastMsgTime() const179269
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::getMsg()193342
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::getMsg()193357
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const193357
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::peekMsg() const193358
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const209353
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::hasMsg() const209357
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)286813
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)286852
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)286880
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)286969
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)294568
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)294651
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)296692
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)296958
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)339245
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const> const&)339919
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const340581
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::getMsg()340679
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::getMsg()340881
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::peekMsg() const340959
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::hasMsg() const344625
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const344633
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const367688
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::lastMsgTime() const367826
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)436262
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const> const&)436706
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)460831
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)461255
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const463643
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::hasMsg() const463654
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::newMsg() const469400
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const469907
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)531539
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)531819
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const644375
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()644418
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::getMsg()644459
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::peekMsg() const644496
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::hasMsg() const825744
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const825821
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)838761
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)838860
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)1025395
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)1025496
+
+
+ + + +
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..fc945a94a5 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.func.html @@ -0,0 +1,4256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - subscribe_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9812081.7 %
Date:2024-04-09 22:03:33Functions:487104446.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&)339919
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::start()324
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&)324
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()324
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2324
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&)339245
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()324
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&)324
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::~Impl().2324
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const> const&)52018
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::start()25
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::getMsg()9517
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&)25
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()25
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().225
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&)52018
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()25
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::getMsg()9517
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&)25
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::~Impl().225
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()164
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&)164
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()164
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2164
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::start()164
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&)164
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::~Impl().2164
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const> const&)286969
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::start()153
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::getMsg()13503
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&)153
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()153
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2153
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&)286880
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()153
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::getMsg()13503
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&)153
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::~Impl().2153
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().20
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::~Impl().20
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const> const&)166923
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::start()86
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()3808
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&)82
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
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&)166897
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()86
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::getMsg()3808
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&)82
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const> const&)531819
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::start()259
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()166942
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&)259
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()259
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2259
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&)531539
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()259
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::getMsg()166896
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&)259
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::~Impl().2259
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)1025395
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::start()486
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()644418
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&)486
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()486
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2486
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&)1025496
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&)8
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::start()486
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::getMsg()644459
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&)486
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::~Impl().2486
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const> const&)838860
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::start()189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::getMsg()229
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&)189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2189
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&)838761
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()189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::getMsg()229
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&)189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::~Impl().2189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const> const&)296692
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::start()167
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::getMsg()151728
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&)167
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()167
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2167
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&)296958
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()167
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::getMsg()151728
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&)167
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::~Impl().2167
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&)294651
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::start()313
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::getMsg()340881
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&)313
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()313
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2313
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&)294568
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()313
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::getMsg()340679
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&)313
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::~Impl().2313
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const> const&)286852
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::start()328
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::getMsg()38330
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&)328
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()328
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2328
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&)286813
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()328
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::getMsg()38345
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&)328
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::~Impl().2328
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrackerCommand_<std::allocator<void> > const> const&)172861
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::start()164
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::getMsg()611
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&)164
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()164
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2164
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&)172906
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()164
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::getMsg()611
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&)164
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::~Impl().2164
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimatorOutput_<std::allocator<void> > const> const&)171703
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::start()85
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&)85
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()85
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().285
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&)171710
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()85
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&)85
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::~Impl().285
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const> const&)19866
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::start()82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::getMsg()549
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
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&)19866
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()82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::getMsg()549
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ObstacleSectors_<std::allocator<void> > const> const&)390
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::start()82
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::getMsg()421
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
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&)390
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()82
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::getMsg()421
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const> const&)294
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&)294
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::start()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::~Impl().210
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&)4597
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::start()271
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::getMsg()19280
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&)271
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()271
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2271
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&)4591
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()271
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::getMsg()19280
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&)271
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::~Impl().2271
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::DynamicsConstraints_<std::allocator<void> > const> const&)12826
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
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&)12826
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()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::~Impl().282
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()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
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&)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()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > const> const&)121249
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()4
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
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&)121249
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()82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::getMsg()4
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > const> const&)65583
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()353
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()61970
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&)353
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()353
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2353
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&)65354
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()353
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::getMsg()61990
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&)353
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::~Impl().2353
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const> const&)1951
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&)1951
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&)532
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()25
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&)25
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()25
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().225
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&)532
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()25
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&)25
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().225
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > const> const&)1509
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
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&)1509
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()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const> const&)3112
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()25
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&)25
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()25
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().225
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&)3112
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()25
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&)25
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::~Impl().225
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
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&)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()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > const> const&)65418
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()353
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::getMsg()135106
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&)353
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()353
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2353
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&)65184
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()353
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::getMsg()135113
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&)353
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().2353
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > const> const&)1510
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
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&)1510
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()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::Impl::~Impl().282
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()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()82
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().282
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::data_callback(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::process_new_message(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const> const&)1
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::default_timeout_callback(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::~Impl().282
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const> const&)11558
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()72
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&)11531
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()72
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&)436706
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::start()246
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::ImplThreadsafe(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)> const&)246
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()246
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2246
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&)436262
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()246
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::Impl(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)> const&)246
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::~Impl().2246
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)461255
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::start()252
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::getMsg()193342
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&)252
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()252
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2252
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&)460831
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()252
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::getMsg()193357
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&)252
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::~Impl().2252
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::data_callback(boost::shared_ptr<std_msgs::Float64_<std::allocator<void> > const> const&)18280
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::start()164
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::getMsg()32190
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&)164
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe()164
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::~ImplThreadsafe().2164
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&)18280
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()164
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::getMsg()32190
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&)164
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::~Impl().2164
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const324
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const9517
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const9517
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::hasMsg() const9517
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::Impl::peekMsg() const9517
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]() const25
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const164
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const16973
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const13498
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::hasMsg() const16970
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::Impl::peekMsg() const13510
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]() const153
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const16411
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const3808
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::lastMsgTime() const5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::hasMsg() const16411
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::Impl::peekMsg() const3808
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]() const83
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const344633
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const166873
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::hasMsg() const344625
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::Impl::peekMsg() const166965
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]() const259
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const825821
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const644375
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]() const108
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::hasMsg() const825744
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::Impl::peekMsg() const644496
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]() const661
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const162
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const17183
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const229
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() const162
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::hasMsg() const17183
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::Impl::peekMsg() const229
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]() const189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const151728
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const151728
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::hasMsg() const151728
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::Impl::peekMsg() const151728
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]() const167
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const367688
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const469907
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const340581
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() const367826
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::newMsg() const469400
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::Impl::peekMsg() const340959
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]() const313
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const59462
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const38345
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::hasMsg() const59471
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::Impl::peekMsg() const38345
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]() const328
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const15
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const611
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::hasMsg() const15
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::Impl::peekMsg() const611
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]() const164
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const85
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const549
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const549
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::hasMsg() const549
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::Impl::peekMsg() const549
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]() const82
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const301
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const2357
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const421
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() const301
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::hasMsg() const2357
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::Impl::peekMsg() const421
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]() const82
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const10
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const17213
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const19280
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]() const82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::hasMsg() const17213
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::Impl::peekMsg() const19280
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]() const353
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const6
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::hasMsg() const6
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::peekMsg() const22
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const4
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const4
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]() const82
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const63699
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const61986
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const63729
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const61989
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]() const353
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const10
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const17005
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const17005
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]() const25
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const78
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() const78
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]() const82
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const25
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const133860
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const135108
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]() const97
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::hasMsg() const133877
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::Impl::peekMsg() const135112
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]() const450
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const78
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() const78
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]() const82
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const72
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const72
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::hasMsg() const72
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::peekMsg() const72
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const9
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const179269
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const209353
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::lastMsgTime() const179269
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::hasMsg() const209357
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::Impl::topicName[abi:cxx11]() const246
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const463643
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const193357
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::usedMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::ImplThreadsafe::topicName[abi:cxx11]() const1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::hasMsg() const463654
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::Impl::peekMsg() const193358
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]() const253
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::lastMsgTime() const32190
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::hasMsg() const46336
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::ImplThreadsafe::peekMsg() const32190
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() const32190
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::hasMsg() const46336
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::Impl::peekMsg() const32190
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]() const164
+
+
+ + + +
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..b049587cc0 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.html @@ -0,0 +1,413 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - subscribe_handler.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9812081.7 %
Date:2024-04-09 22:03:33Functions:487104446.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        5359 :     Impl(const SubscribeHandlerOptions& options, const message_callback_t& message_callback = message_callback_t())
+      29        5359 :         : m_nh(options.nh),
+      30        5359 :           m_topic_name(options.topic_name),
+      31        5359 :           m_node_name(options.node_name),
+      32             :           m_got_data(false),
+      33             :           m_new_data(false),
+      34             :           m_used_data(false),
+      35        5359 :           m_timeout_manager(options.timeout_manager),
+      36             :           m_latest_message_time(0),
+      37             :           m_latest_message(nullptr),
+      38             :           m_message_callback(message_callback),
+      39        5359 :           m_queue_size(options.queue_size),
+      40        5359 :           m_transport_hints(options.transport_hints)
+      41             :     {
+      42        5359 :       if (options.no_message_timeout != mrs_lib::no_timeout)
+      43             :       {
+      44             :         // initialize a new TimeoutManager if not provided by the user
+      45          68 :         if (!m_timeout_manager)
+      46          68 :           m_timeout_manager = std::make_shared<mrs_lib::TimeoutManager>(m_nh, ros::Rate(2.0/options.no_message_timeout.toSec()));
+      47             : 
+      48             :         // initialize the callback for the TimeoutManager
+      49         136 :         std::function<void(const ros::Time&)> timeout_mgr_callback;
+      50          68 :         if (options.timeout_callback)
+      51           1 :           timeout_mgr_callback = std::bind(options.timeout_callback, topicName(), std::placeholders::_1);
+      52             :         else
+      53          67 :           timeout_mgr_callback = std::bind(&Impl::default_timeout_callback, this, topicName(), std::placeholders::_1);
+      54             : 
+      55             :         // register the timeout callback with the TimeoutManager
+      56          68 :         m_timeout_id = m_timeout_manager->registerNew(options.no_message_timeout, timeout_mgr_callback);
+      57             :       }
+      58             : 
+      59       16077 :       const std::string msg = "Subscribed to topic '" + m_topic_name + "' -> '" + topicName() + "'";
+      60        5359 :       if (m_node_name.empty())
+      61           1 :         ROS_INFO_STREAM(msg);
+      62             :       else
+      63        5358 :         ROS_INFO_STREAM("[" << m_node_name << "]: " << msg);
+      64        5359 :     }
+      65             :     //}
+      66             : 
+      67        5359 :     virtual ~Impl() = default;
+      68             : 
+      69             :   public:
+      70             :     /* getMsg() method //{ */
+      71     1812773 :     virtual typename MessageType::ConstPtr getMsg()
+      72             :     {
+      73     1812773 :       m_new_data = false;
+      74     1812773 :       m_used_data = true;
+      75     1812773 :       return peekMsg();
+      76             :     }
+      77             :     //}
+      78             : 
+      79             :     /* peekMsg() method //{ */
+      80     1813165 :     virtual typename MessageType::ConstPtr peekMsg() const
+      81             :     {
+      82             :       /* assert(m_got_data); */
+      83             :       /* if (!m_got_data) */
+      84             :       /*   ROS_ERROR("[%s]: No data received yet from topic '%s' (forgot to check hasMsg()?)! Returning empty message.", m_node_name.c_str(), */
+      85             :       /*             topicName().c_str()); */
+      86     1813165 :       return m_latest_message;
+      87             :     }
+      88             :     //}
+      89             : 
+      90             :     /* hasMsg() method //{ */
+      91     2395901 :     virtual bool hasMsg() const
+      92             :     {
+      93     2395901 :       return m_got_data;
+      94             :     }
+      95             :     //}
+      96             : 
+      97             :     /* newMsg() method //{ */
+      98      469400 :     virtual bool newMsg() const
+      99             :     {
+     100      469400 :       return m_new_data;
+     101             :     }
+     102             :     //}
+     103             : 
+     104             :     /* usedMsg() method //{ */
+     105           0 :     virtual bool usedMsg() const
+     106             :     {
+     107           0 :       return m_used_data;
+     108             :     }
+     109             :     //}
+     110             : 
+     111             :     /* waitForNew() method //{ */
+     112           0 :     virtual typename MessageType::ConstPtr waitForNew(const ros::WallDuration& timeout)
+     113             :     {
+     114             :       // convert the ros type to chrono type
+     115           0 :       const std::chrono::duration<float> chrono_timeout(timeout.toSec());
+     116             :       // lock the mutex guarding the m_new_data flag
+     117           0 :       std::unique_lock lock(m_new_data_mtx);
+     118             :       // if new data is available, return immediately, otherwise attempt to wait for new data using the respective condition variable
+     119           0 :       if (m_new_data)
+     120           0 :         return getMsg();
+     121           0 :       else if (m_new_data_cv.wait_for(lock, chrono_timeout) == std::cv_status::no_timeout && m_new_data)
+     122           0 :         return getMsg();
+     123             :       else
+     124           0 :         return nullptr;
+     125             :     };
+     126             :     //}
+     127             : 
+     128             :     /* lastMsgTime() method //{ */
+     129      579909 :     virtual ros::Time lastMsgTime() const
+     130             :     {
+     131      579909 :       return m_latest_message_time;
+     132             :     };
+     133             :     //}
+     134             : 
+     135             :     /* topicName() method //{ */
+     136        5715 :     virtual std::string topicName() const
+     137             :     {
+     138        5715 :       std::string ret = m_sub.getTopic();
+     139        5715 :       if (ret.empty())
+     140        5427 :         ret = m_nh.resolveName(m_topic_name);
+     141        5715 :       return ret;
+     142             :     }
+     143             :     //}
+     144             : 
+     145             :     /* start() method //{ */
+     146        5363 :     virtual void start()
+     147             :     {
+     148        5363 :       if (m_timeout_manager)
+     149          72 :         m_timeout_manager->start(m_timeout_id);
+     150        5363 :       m_sub = m_nh.subscribe(m_topic_name, m_queue_size, &Impl::data_callback, this, m_transport_hints);
+     151        5363 :     }
+     152             :     //}
+     153             : 
+     154             :     /* stop() method //{ */
+     155           5 :     virtual void stop()
+     156             :     {
+     157           5 :       if (m_timeout_manager)
+     158           5 :         m_timeout_manager->pause(m_timeout_id);
+     159           5 :       m_sub.shutdown();
+     160           5 :     }
+     161             :     //}
+     162             : 
+     163             :   protected:
+     164             :     ros::NodeHandle m_nh;
+     165             :     ros::Subscriber m_sub;
+     166             : 
+     167             :   protected:
+     168             :     std::string m_topic_name;
+     169             :     std::string m_node_name;
+     170             : 
+     171             :   protected:
+     172             :     bool m_got_data;   // whether any data was received
+     173             : 
+     174             :     mutable std::mutex m_new_data_mtx;
+     175             :     mutable std::condition_variable m_new_data_cv;
+     176             :     bool m_new_data;   // whether new data was received since last call to get_data
+     177             : 
+     178             :     bool m_used_data;  // whether get_data was successfully called at least once
+     179             : 
+     180             :   protected:
+     181             :     std::shared_ptr<mrs_lib::TimeoutManager> m_timeout_manager;
+     182             :     mrs_lib::TimeoutManager::timeout_id_t m_timeout_id;
+     183             : 
+     184             :   protected:
+     185             :     ros::Time m_latest_message_time;
+     186             :     typename MessageType::ConstPtr m_latest_message;
+     187             :     message_callback_t m_message_callback;
+     188             : 
+     189             :   private:
+     190             :     uint32_t m_queue_size;
+     191             :     ros::TransportHints m_transport_hints;
+     192             : 
+     193             :   protected:
+     194             :     /* default_timeout_callback() method //{ */
+     195           8 :     void default_timeout_callback(const std::string& topic_name, const ros::Time& last_msg)
+     196             :     {
+     197           8 :       const ros::Duration since_msg = (ros::Time::now() - last_msg);
+     198           8 :       const auto n_pubs = m_sub.getNumPublishers();
+     199          24 :       const std::string txt = "Did not receive any message from topic '" + topic_name + "' for " + std::to_string(since_msg.toSec()) + "s ("
+     200             :                               + std::to_string(n_pubs) + " publishers on this topic)";
+     201           8 :       if (m_node_name.empty())
+     202           0 :         ROS_WARN_STREAM(txt);
+     203             :       else
+     204           8 :         ROS_WARN_STREAM("[" << m_node_name << "]: " << txt);
+     205           8 :     }
+     206             :     //}
+     207             : 
+     208             :     /* process_new_message() method //{ */
+     209     5689064 :     void process_new_message(const typename MessageType::ConstPtr& msg)
+     210             :     {
+     211     5689064 :       m_latest_message_time = ros::Time::now();
+     212     5692743 :       m_latest_message = msg;
+     213             :       // If the message callback is registered, the new data will immediately be processed,
+     214             :       // so reset the flag. Otherwise, set the flag.
+     215     5690768 :       m_new_data = !m_message_callback;
+     216     5686055 :       m_got_data = true;
+     217     5686055 :       m_new_data_cv.notify_one();
+     218     5693208 :     }
+     219             :     //}
+     220             : 
+     221             :     /* data_callback() method //{ */
+     222           0 :     virtual void data_callback(const typename MessageType::ConstPtr& msg)
+     223             :     {
+     224             :       {
+     225           0 :         std::lock_guard lck(m_new_data_mtx);
+     226           0 :         if (m_timeout_manager)
+     227           0 :           m_timeout_manager->reset(m_timeout_id);
+     228           0 :         process_new_message(msg);
+     229             :       }
+     230             : 
+     231             :       // execute the callback after unlocking the mutex to enable multi-threaded callback execution
+     232           0 :       if (m_message_callback)
+     233           0 :         m_message_callback(msg);
+     234           0 :     }
+     235             :     //}
+     236             :   };
+     237             :   //}
+     238             : 
+     239             :   /* SubscribeHandler_threadsafe class //{ */
+     240             :   template <typename MessageType>
+     241             :   class SubscribeHandler<MessageType>::ImplThreadsafe : public SubscribeHandler<MessageType>::Impl
+     242             :   {
+     243             :   private:
+     244             :     using impl_class_t = SubscribeHandler<MessageType>::Impl;
+     245             : 
+     246             :   public:
+     247             :     using timeout_callback_t = typename impl_class_t::timeout_callback_t;
+     248             :     using message_callback_t = typename impl_class_t::message_callback_t;
+     249             : 
+     250             :     friend class SubscribeHandler<MessageType>;
+     251             : 
+     252             :   public:
+     253        5359 :     ImplThreadsafe(const SubscribeHandlerOptions& options, const message_callback_t& message_callback = message_callback_t())
+     254        5359 :         : impl_class_t::Impl(options, message_callback)
+     255             :     {
+     256        5359 :     }
+     257             : 
+     258             :   public:
+     259     2395918 :     virtual bool hasMsg() const override
+     260             :     {
+     261     4791673 :       std::lock_guard lck(m_mtx);
+     262     4791773 :       return impl_class_t::hasMsg();
+     263             :     }
+     264      469907 :     virtual bool newMsg() const override
+     265             :     {
+     266      939073 :       std::lock_guard lck(m_mtx);
+     267      938014 :       return impl_class_t::newMsg();
+     268             :     }
+     269           0 :     virtual bool usedMsg() const override
+     270             :     {
+     271           0 :       std::lock_guard lck(m_mtx);
+     272           0 :       return impl_class_t::usedMsg();
+     273             :     }
+     274     1812923 :     virtual typename MessageType::ConstPtr getMsg() override
+     275             :     {
+     276     3625015 :       std::lock_guard lck(m_mtx);
+     277     3625124 :       return impl_class_t::getMsg();
+     278             :     }
+     279     1812554 :     virtual typename MessageType::ConstPtr peekMsg() const override
+     280             :     {
+     281     3624936 :       std::lock_guard lck(m_mtx);
+     282     3624656 :       return impl_class_t::peekMsg();
+     283             :     }
+     284      579771 :     virtual ros::Time lastMsgTime() const override
+     285             :     {
+     286     1159608 :       std::lock_guard lck(m_mtx);
+     287     1159526 :       return impl_class_t::lastMsgTime();
+     288             :     };
+     289         288 :     virtual std::string topicName() const override
+     290             :     {
+     291         576 :       std::lock_guard lck(m_mtx);
+     292         576 :       return impl_class_t::topicName();
+     293             :     };
+     294        5363 :     virtual void start() override
+     295             :     {
+     296       10726 :       std::lock_guard lck(m_mtx);
+     297       10726 :       return impl_class_t::start();
+     298             :     }
+     299           5 :     virtual void stop() override
+     300             :     {
+     301          10 :       std::lock_guard lck(m_mtx);
+     302          10 :       return impl_class_t::stop();
+     303             :     }
+     304             : 
+     305       10718 :     virtual ~ImplThreadsafe() override = default;
+     306             : 
+     307             :   protected:
+     308     5691299 :     virtual void data_callback(const typename MessageType::ConstPtr& msg) override
+     309             :     {
+     310             :       {
+     311    11381618 :         std::scoped_lock lck(m_mtx, this->m_new_data_mtx);
+     312     5673124 :         if (this->m_timeout_manager)
+     313      138729 :           this->m_timeout_manager->reset(this->m_timeout_id);
+     314     5648902 :         impl_class_t::process_new_message(msg);
+     315             :       }
+     316             : 
+     317             :       // execute the callback after unlocking the mutex to enable multi-threaded callback execution
+     318     5673363 :       if (this->m_message_callback)
+     319     3170740 :         impl_class_t::m_message_callback(msg);
+     320     5666161 :     }
+     321             : 
+     322             :   private:
+     323             :     mutable std::recursive_mutex m_mtx;
+     324             :   };
+     325             :   //}
+     326             : 
+     327             : }  // namespace mrs_lib
+     328             : 
+     329             : #endif  // SUBSCRIBE_HANDLER_HPP
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.overview.html b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.overview.html new file mode 100644 index 0000000000..02a4b3f18a --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.overview.html @@ -0,0 +1,103 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.png b/mrs_lib/include/mrs_lib/impl/subscribe_handler.hpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..039a0b123a5be30b6b86ab27f511bc7c3edec5e5 GIT binary patch literal 1372 zcmV-i1*7_jP)PbXF zRA_$hj4@iQ$tY%%8HH^zS zW6cEpSWW-;D~SoWLPH?$vi4?$WWPqq&<)@QNwyBXCKhLurC>WWYYu4IJlu$ zh0YNwZba1sMI@ur1}1BoodzD`chMU0CaKq2J9ixq7b9FNBgj=2+oknPWgKADO5G|$ z>eIT=*2XV9fVDuGImIaQ6XuE50Sgsx4$L#=nuacVL?du^pqon%e$h)2Vvk6+u8nZs zjx+Zqpu{l^MF&#YAg*mDmw38zD>niv>=>g1Y{)AkW3Alcmgy#!WyFrOy_0myUJ6_8 zl??RO5dbY8Iw~0t^PG{`_6CpnB$bS^2Cw5*r3_Ao`HaTz5^*V>WB}_WkzB(11Pgh{ zwF5T;n+?!GY1~KxbH>ImZg*A98Oz(|zj=OvdFBhi?Y8gxW8d$`m;bmC z;{ZOWe`|ra9q8@cD{_kOGtV@Bl6jIsJAQnWUo^@@-Ue2%Sn>~9-!O~45hB|+0PhF# zktMx@bVJXyga{Yj1HtF|Ka}ySkF8iDdwn{0W;U4X8z7{D=D(l^)j;B_@EHb zdvD;*W_H=wUCK(Siv5XYk4v0A>u)c6({-u&<7$5~OX^|8?%vajS5_rekW=7_^&h}#8KHO$DWFb9 z$XyNU*`aiHp)yKF7hT|1BiGss3zUS?X)AYbSqF9y1t0^&LcR*`syj6#D~ zhi2rCwpMspEJoTa1-O{QlrW!JDi?GDnS1Zsr*&W)k!5&&My;fM5F zeG#ZYx^-W(*vofU8BnJ+I>@2Ysb?|(H`@!F)+e#Ysy5BPqfC2-M0L=$XGmpp<+ixJ zJ{B26<*ex|2(_txg=g5~eRQwlBg&BXq}0lwBaMNTKB5fS+!vmG3=bf_rVLe7D3!6- zDyl}nw<|-RO6iwo_M+4$lzEx8c%XPp4|e2A-Nr6TAaV4>F{tgYn#RyYn&o;lW?FG= zwXUZO!Ft0ayWtv&90CDPudjtwT&FZWxrWM&fbYm3<;_%-^cyno{9aucTl%v9o+2CG eIJickaQ^@zmyh{xOPAyT0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/timer.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - timer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:141687.5 %
Date:2024-04-09 22:03:33Functions: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..b53463ea58 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/timer.hpp.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/timer.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - timer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:141687.5 %
Date:2024-04-09 22:03:33Functions: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..5eeb98d632 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/timer.hpp.gcov.html @@ -0,0 +1,179 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/timer.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - timer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:141687.5 %
Date:2024-04-09 22:03:33Functions: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             : 
+      41             :   friend class ThreadTimer;
+      42             : 
+      43             :   // to keep rule of five since we have a custom destructor
+      44             :   Impl(const Impl&) = delete;
+      45             :   Impl(Impl&&) = delete;
+      46             :   Impl& operator=(const Impl&) = delete;
+      47             :   Impl& operator=(Impl&&) = delete;
+      48             : 
+      49             : private:
+      50             :   std::thread thread_;
+      51             :   std::function<void(const ros::TimerEvent&)> callback_;
+      52             : 
+      53             :   bool oneshot_;
+      54             : 
+      55             :   bool breakableSleep(const ros::Time& until);
+      56             :   void threadFcn();
+      57             : 
+      58             :   std::mutex mutex_wakeup_;
+      59             :   std::condition_variable wakeup_cond_;
+      60             :   std::recursive_mutex mutex_state_;
+      61             :   bool running_;
+      62             :   ros::Duration delay_dur_;
+      63             :   bool ending_;
+      64             :   ros::Time next_expected_;
+      65             :   ros::Time last_expected_;
+      66             :   ros::Time last_real_;
+      67             : 
+      68             : };
+      69             : 
+      70             : //}
+      71             : 
+      72             : /* ThreadTimer constructors and destructors//{ */
+      73             : 
+      74             : template <class ObjectType>
+      75           8 : ThreadTimer::ThreadTimer([[maybe_unused]] const ros::NodeHandle& nh, const ros::Rate& rate, void (ObjectType::*const callback)(const ros::TimerEvent&),
+      76             :                          ObjectType* const obj, const bool oneshot, const bool autostart)
+      77           8 :   : ThreadTimer(nh, rate.expectedCycleTime(), callback, obj, oneshot, autostart)
+      78             : {
+      79           8 : }
+      80             : 
+      81             : template <class ObjectType>
+      82           8 : ThreadTimer::ThreadTimer([[maybe_unused]] const ros::NodeHandle& nh, const ros::Duration& duration,
+      83           8 :                          void (ObjectType::*const callback)(const ros::TimerEvent&), ObjectType* const obj, bool oneshot, const bool autostart) 
+      84             : {
+      85           8 :   const auto cbk = std::bind(callback, obj, std::placeholders::_1);
+      86           8 :   if (duration == ros::Duration(0))
+      87           0 :     oneshot = true;
+      88           8 :   this->impl_ = std::make_unique<Impl>(cbk, duration, oneshot);
+      89           8 :   if (autostart)
+      90           0 :     this->impl_->start();
+      91           8 : }
+      92             : 
+      93             : //}
+      94             : 
+      95             : #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..b7e96440c1 --- /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..3a8f0b8378ef00e8147fa63b03c2ae9387250e7f GIT binary patch literal 527 zcmV+q0`UEbP)JN zR7i={R@)VWAPglNzzVVfuHXoD|5d7xK!QgI7C)M{>Va!+9wd!%&RG}5nx?3MYH2mKoS>NR(V4pexZ8zpDW;uZNe9!VlV2nV^fG~`kRhm|F zi$qoux6G_F!QC;67GAA$~bb?j*aS0!Or+(;H2D4Zlb5g7yb)bo^6@Po)e+E zv2lhZ4>NI^h(pUNMg=cMosb3(d@@TYYJ^UXJO?E8l=8wj^Ky6s2aEfFGZ-&6?s*Z@ zkb!CL3)RL&8l}blAEWEhYu&%6 literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/impl/transformer.hpp.func-sort-c.html b/mrs_lib/include/mrs_lib/impl/transformer.hpp.func-sort-c.html new file mode 100644 index 0000000000..1dcdd46a98 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/transformer.hpp.func-sort-c.html @@ -0,0 +1,272 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/transformer.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - transformer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:556485.9 %
Date:2024-04-09 22:03:33Functions:404883.3 %
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::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&)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
void mrs_lib::Transformer::setHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > 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
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&)471
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&)20515
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&)20515
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)20515
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&)20863
void mrs_lib::Transformer::setHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)20863
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&)81599
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&)83871
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&)144228
void mrs_lib::Transformer::setHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)144228
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&)191816
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&)212373
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&)212623
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&)212679
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&)233463
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&)534780
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&)535198
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&)565653
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&)636590
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&)636778
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&)636823
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&)637380
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&)637383
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&)679268
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&)709882
+
+
+ + + +
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..ce5a1b91ca --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/transformer.hpp.func.html @@ -0,0 +1,272 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/transformer.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - transformer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:556485.9 %
Date:2024-04-09 22:03:33Functions:404883.3 %
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&)565653
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&)191816
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&)637383
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&)709882
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&)212679
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&)637380
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&)144228
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&)20863
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&)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<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&)535198
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&)534780
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&)212373
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&)212623
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&)636778
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&)636590
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&)20515
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&)20515
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&)679268
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> > const&)233463
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&)636823
std_msgs::Header_<std::allocator<void> > mrs_lib::Transformer::getHeader<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)20515
void mrs_lib::Transformer::setHeader<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)144228
void mrs_lib::Transformer::setHeader<geometry_msgs::PointStamped_<std::allocator<void> > >(geometry_msgs::PointStamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)20863
void mrs_lib::Transformer::setHeader<geometry_msgs::Vector3Stamped_<std::allocator<void> > >(geometry_msgs::Vector3Stamped_<std::allocator<void> >&, std_msgs::Header_<std::allocator<void> > const&)0
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&)83871
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&)471
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&)81599
+
+
+ + + +
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..e447d2cef8 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/transformer.hpp.gcov.html @@ -0,0 +1,280 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/transformer.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - transformer.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:556485.9 %
Date:2024-04-09 22:03:33Functions:404883.3 %
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     1570069 :   std_msgs::Header Transformer::getHeader(const msg_t& msg)
+      15             :   {
+      16     1570069 :     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      165091 :   void Transformer::setHeader(msg_t& msg, const std_msgs::Header& header)
+      33             :   {
+      34      165091 :     msg.header = header;
+      35      165091 :   }
+      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      165091 :   T Transformer::copyChangeFrame(const T& what, const std::string& frame_id)
+      49             :   {
+      50      165091 :     T ret = what;
+      51             :     if constexpr (has_header_member_v<T>)
+      52             :     {
+      53      330182 :       std_msgs::Header new_header = getHeader(what);
+      54      165091 :       new_header.frame_id = frame_id;
+      55      165091 :       setHeader(ret, new_header);
+      56             :     }
+      57      165091 :     return ret;
+      58             :   }
+      59             : 
+      60             :   //}
+      61             : 
+      62             :   /* transformImpl() //{ */
+      63             : 
+      64             :   template <class T>
+      65     1559984 :   std::optional<T> Transformer::transformImpl(const geometry_msgs::TransformStamped& tf, const T& what)
+      66             :   {
+      67     3119958 :     const std::string from_frame = frame_from(tf);
+      68     3119939 :     const std::string to_frame = frame_to(tf);
+      69             : 
+      70     1559978 :     if (from_frame == to_frame)
+      71      165091 :       return copyChangeFrame(what, from_frame);
+      72             : 
+      73     2789770 :     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      757487 :       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      757486 :       else if (to_frame == latlon_frame_name)
+      89             :       {
+      90        3584 :         const std::optional<T> tmp = doTransform(what, tf);
+      91        1793 :         if (!tmp.has_value())
+      92           0 :           return std::nullopt;
+      93        1793 :         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      637409 :       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     1393090 :     return doTransform(what, tf);
+     108             :   }
+     109             : 
+     110             :   //}
+     111             : 
+     112             :   /* doTransform() //{ */
+     113             : 
+     114             :   template <class T>
+     115     1394893 :   std::optional<T> Transformer::doTransform(const T& what, const geometry_msgs::TransformStamped& tf)
+     116             :   {
+     117             :     try
+     118             :     {
+     119     2789701 :       T result;
+     120     1394877 :       tf2::doTransform(what, result, tf);
+     121     1394888 :       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     1404508 :   std::optional<T> Transformer::transformSingle(const T& what, const std::string& to_frame_raw)
+     139             :   {
+     140     2810263 :     const std_msgs::Header orig_header = getHeader(what);
+     141     2809744 :     return transformSingle(orig_header.frame_id, what, to_frame_raw, orig_header.stamp);
+     142             :   }
+     143             : 
+     144             :   template <class T>
+     145     1404865 :   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     2810611 :     std::scoped_lock lck(mutex_);
+     148             : 
+     149     1405767 :     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     2811516 :     const std::string from_frame = resolveFrameImpl(from_frame_raw);
+     156     2811486 :     const std::string to_frame = resolveFrameImpl(to_frame_raw);
+     157     2811501 :     const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
+     158             : 
+     159             :     // get the transform
+     160     2811520 :     const auto tf_opt = getTransformImpl(from_frame, to_frame, time_stamp, latlon_frame);
+     161     1405763 :     if (!tf_opt.has_value())
+     162       11763 :       return std::nullopt;
+     163     1393992 :     const geometry_msgs::TransformStamped& tf = tf_opt.value();
+     164             : 
+     165             :     // do the transformation
+     166     2787977 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     167     1393999 :     return transformImpl(tf_resolved, what);
+     168             :   }
+     169             : 
+     170             :   //}
+     171             : 
+     172             :   /* transform() //{ */
+     173             : 
+     174             :   template <class T>
+     175      165959 :   std::optional<T> Transformer::transform(const T& what, const geometry_msgs::TransformStamped& tf)
+     176             :   {
+     177      331918 :     std::scoped_lock lck(mutex_);
+     178             : 
+     179      165959 :     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      331918 :     const std::string from_frame = resolveFrameImpl(frame_from(tf));
+     186      331918 :     const std::string to_frame = resolveFrameImpl(frame_to(tf));
+     187      331918 :     const geometry_msgs::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
+     188             : 
+     189      165959 :     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..9a5816b2b2 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/ukf.hpp.func-sort-c.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/ukf.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - ukf.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7710474.0 %
Date:2024-04-09 22:03:33Functions: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..ba554fd6de --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/ukf.hpp.func.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/ukf.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - ukf.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7710474.0 %
Date:2024-04-09 22:03:33Functions: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..9e6d057e0b --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/ukf.hpp.gcov.html @@ -0,0 +1,364 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/ukf.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - ukf.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7710474.0 %
Date:2024-04-09 22:03:33Functions: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..4c9af60124 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.func-sort-c.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/vector_converter.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - vector_converter.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1818100.0 %
Date:2024-04-09 22:03:33Functions: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..929dd9807f --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.func.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/vector_converter.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - vector_converter.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1818100.0 %
Date:2024-04-09 22:03:33Functions: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..b436f25922 --- /dev/null +++ b/mrs_lib/include/mrs_lib/impl/vector_converter.hpp.gcov.html @@ -0,0 +1,173 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/impl/vector_converter.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/impl - vector_converter.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1818100.0 %
Date:2024-04-09 22:03:33Functions:1818100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
ukf.h +
0.0%
+ 		0.0 %0 / 40.0 %0 / 2
dynamic_reconfigure_mgr.h +
65.5%65.5%
+ 		65.5 %57 / 8732.9 %28 / 85
<unnamed>65.5 %57 / 8732.9 %28 / 85
scope_timer.h +
33.3%33.3%
+ 		33.3 %2 / 633.3 %1 / 3
<unnamed>33.3 %2 / 633.3 %1 / 3
msg_extractor.h +
33.3%33.3%
+ 		33.3 %33 / 9934.1 %15 / 44
<unnamed>33.3 %33 / 9934.1 %15 / 44
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11145.5 %20 / 44
<unnamed>90.1 %100 / 11145.5 %20 / 44
subscribe_handler.h +
89.8%89.8%
+ 		89.8 %44 / 4952.2 %387 / 741
<unnamed>89.8 %44 / 4952.2 %387 / 741
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5453.2 %25 / 47
<unnamed>81.5 %44 / 5453.2 %25 / 47
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
<unnamed>85.0 %51 / 6077.8 %14 / 18
attitude_converter.h +
85.2%85.2%
+ 		85.2 %23 / 2781.8 %9 / 11
<unnamed>85.2 %23 / 2781.8 %9 / 11
mutex.h +
100.0%
+ 		100.0 %11 / 1182.5 %66 / 80
<unnamed>100.0 %11 / 1182.5 %66 / 80
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
<unnamed>100.0 %4 / 485.7 %6 / 7
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.2 %118 / 119
<unnamed>100.0 %4 / 499.2 %118 / 119
timeout_manager.h +
100.0%
+ 		100.0 %4 / 4-0 / 0
<unnamed>100.0 %4 / 4-0 / 0
visual_object.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
<unnamed>100.0 %2 / 2100.0 %1 / 1
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
<unnamed>100.0 %4 / 4100.0 %2 / 2
gps_conversions.h +
76.5%76.5%
+ 		76.5 %104 / 136100.0 %4 / 4
<unnamed>76.5 %104 / 136100.0 %4 / 4
utils.h +
100.0%
+ 		100.0 %15 / 15100.0 %5 / 5
<unnamed>100.0 %15 / 15100.0 %5 / 5
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %18 / 18
<unnamed>100.0 %3 / 3100.0 %18 / 18
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..c4c3b15aed --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-detail-sort-l.html @@ -0,0 +1,444 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_libHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:78198579.3 %
Date:2024-04-09 22:03:33Functions:833134561.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
ukf.h +
0.0%
+ 		0.0 %0 / 40.0 %0 / 2
dynamic_reconfigure_mgr.h +
65.5%65.5%
+ 		65.5 %57 / 8732.9 %28 / 85
scope_timer.h +
33.3%33.3%
+ 		33.3 %2 / 633.3 %1 / 3
msg_extractor.h +
33.3%33.3%
+ 		33.3 %33 / 9934.1 %15 / 44
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11145.5 %20 / 44
subscribe_handler.h +
89.8%89.8%
+ 		89.8 %44 / 4952.2 %387 / 741
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5453.2 %25 / 47
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
attitude_converter.h +
85.2%85.2%
+ 		85.2 %23 / 2781.8 %9 / 11
mutex.h +
100.0%
+ 		100.0 %11 / 1182.5 %66 / 80
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.2 %118 / 119
timeout_manager.h +
100.0%
+ 		100.0 %4 / 4-0 / 0
visual_object.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
gps_conversions.h +
76.5%76.5%
+ 		76.5 %104 / 136100.0 %4 / 4
utils.h +
100.0%
+ 		100.0 %15 / 15100.0 %5 / 5
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %18 / 18
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..b3fb9b9e1a --- /dev/null +++ b/mrs_lib/include/mrs_lib/index-sort-l.html @@ -0,0 +1,292 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_libHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:78198579.3 %
Date:2024-04-09 22:03:33Functions:833134561.9 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
ukf.h +
0.0%
+ 		0.0 %0 / 40.0 %0 / 2
scope_timer.h +
33.3%33.3%
+ 		33.3 %2 / 633.3 %1 / 3
msg_extractor.h +
33.3%33.3%
+ 		33.3 %33 / 9934.1 %15 / 44
dynamic_reconfigure_mgr.h +
65.5%65.5%
+ 		65.5 %57 / 8732.9 %28 / 85
gps_conversions.h +
76.5%76.5%
+ 		76.5 %104 / 136100.0 %4 / 4
lkf.h +
81.5%81.5%
+ 		81.5 %44 / 5453.2 %25 / 47
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
attitude_converter.h +
85.2%85.2%
+ 		85.2 %23 / 2781.8 %9 / 11
subscribe_handler.h +
89.8%89.8%
+ 		89.8 %44 / 4952.2 %387 / 741
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11145.5 %20 / 44
param_loader.h +
90.3%90.3%
+ 		90.3 %271 / 300100.0 %78 / 78
visual_object.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %18 / 18
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.2 %118 / 119
timeout_manager.h +
100.0%
+ 		100.0 %4 / 4-0 / 0
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 / 1182.5 %66 / 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..f06530ff84 --- /dev/null +++ b/mrs_lib/include/mrs_lib/index.html @@ -0,0 +1,292 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_libHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:78198579.3 %
Date:2024-04-09 22:03:33Functions:833134561.9 %
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 / 1182.5 %66 / 80
param_loader.h +
90.3%90.3%
+ 		90.3 %271 / 300100.0 %78 / 78
publisher_handler.h +
100.0%
+ 		100.0 %4 / 499.2 %118 / 119
quadratic_throttle_model.h +
100.0%
+ 		100.0 %4 / 4100.0 %2 / 2
repredictor.h +
90.1%90.1%
+ 		90.1 %100 / 11145.5 %20 / 44
scope_timer.h +
33.3%33.3%
+ 		33.3 %2 / 633.3 %1 / 3
service_client_handler.h +
100.0%
+ 		100.0 %3 / 3100.0 %18 / 18
subscribe_handler.h +
89.8%89.8%
+ 		89.8 %44 / 4952.2 %387 / 741
timeout_manager.h +
100.0%
+ 		100.0 %4 / 4-0 / 0
timer.h +
100.0%
+ 		100.0 %4 / 485.7 %6 / 7
transformer.h +
85.0%85.0%
+ 		85.0 %51 / 6077.8 %14 / 18
ukf.h +
0.0%
+ 		0.0 %0 / 40.0 %0 / 2
utils.h +
100.0%
+ 		100.0 %15 / 15100.0 %5 / 5
vector_converter.h +
100.0%
+ 		100.0 %5 / 5100.0 %36 / 36
visual_object.h +
100.0%
+ 		100.0 %2 / 2100.0 %1 / 1
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/lkf.h.func-sort-c.html b/mrs_lib/include/mrs_lib/lkf.h.func-sort-c.html new file mode 100644 index 0000000000..04d85f5d6d --- /dev/null +++ b/mrs_lib/include/mrs_lib/lkf.h.func-sort-c.html @@ -0,0 +1,268 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/lkf.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - lkf.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445481.5 %
Date:2024-04-09 22:03:33Functions: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&)85
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&)146
mrs_lib::LKF<2, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)5834
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&) const5834
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&) const5834
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&) const5834
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&)16429
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)16429
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) const16429
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) const155347
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&)155409
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)155423
mrs_lib::LKF<6, 2, 2>::invert_W(Eigen::Matrix<double, 2, 2, 0, 2, 2>)178502
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&) const178750
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&) const178807
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&) const178855
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) const266485
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&)266968
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)267150
mrs_lib::LKF<3, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)410493
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&) const411949
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&) const411985
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&) const412002
+
+
+ + + +
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..6ea4cf3644 --- /dev/null +++ b/mrs_lib/include/mrs_lib/lkf.h.func.html @@ -0,0 +1,268 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/lkf.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - lkf.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445481.5 %
Date:2024-04-09 22:03:33Functions: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&)16429
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)16429
mrs_lib::LKF<2, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)5834
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&)266968
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)267150
mrs_lib::LKF<3, 1, 1>::invert_W(Eigen::Matrix<double, 1, 1, 0, 1, 1>)410493
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&)146
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&)155409
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)155423
mrs_lib::LKF<6, 2, 2>::invert_W(Eigen::Matrix<double, 2, 2, 0, 2, 2>)178502
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&)85
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) const16429
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&) const5834
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&) const5834
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&) const5834
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&) const411985
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&) const411949
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&) const412002
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) const266485
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&) const178807
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&) const178855
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&) const178750
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) const155347
+
+
+ + + +
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..f3e792aa3b --- /dev/null +++ b/mrs_lib/include/mrs_lib/lkf.h.gcov.html @@ -0,0 +1,460 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/lkf.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - lkf.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445481.5 %
Date:2024-04-09 22:03:33Functions: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         231 :     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      596586 :     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      596586 :       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      421832 :     virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, [[maybe_unused]] double dt) const override
+     139             :     {
+     140      421832 :       statecov_t ret;
+     141      421302 :       ret.x = state_predict(A, sc.x, B, u);
+     142      421442 :       ret.P = covariance_predict(A, sc.P, Q, dt);
+     143      421514 :       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      439002 :     static P_t covariance_predict(const A_t& A, const P_t& P, const Q_t& Q, const double dt)
+     155             :     {
+     156      439002 :       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      438806 :     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      438806 :       return A * x + B * u;
+     172             :     }
+     173             :     //}
+     174             : 
+     175             :   protected:
+     176             :     /* invert_W() method //{ */
+     177      594829 :     static R_t invert_W(R_t W)
+     178             :     {
+     179      594829 :       Eigen::ColPivHouseholderQR<R_t> qr(W);
+     180      596369 :       if (!qr.isInvertible())
+     181             :       {
+     182             :         // add some stuff to the tmp matrix diagonal to make it invertible
+     183       39029 :         R_t ident = R_t::Identity(W.rows(), W.cols());
+     184       39022 :         W += 1e-9 * ident;
+     185       39046 :         qr.compute(W);
+     186       39061 :         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      594482 :       const R_t W_inv = qr.inverse();
+     193     1192104 :       return W_inv;
+     194             :     }
+     195             :     //}
+     196             : 
+     197             :     /* computeKalmanGain() method //{ */
+     198      596638 :     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      596638 :       const R_t W = H * sc.P * H.transpose() + R;
+     202      595983 :       const R_t W_inv = invert_W(W);
+     203      596239 :       const K_t K = sc.P * H.transpose() * W_inv;
+     204     1191454 :       return K;
+     205             :     }
+     206             :     //}
+     207             : 
+     208             :     /* correction_impl() method //{ */
+     209             :     template<int check=n>
+     210      596626 :     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      596626 :       statecov_t ret;
+     214      596225 :       const K_t K = computeKalmanGain(sc, z, R, H);
+     215      595410 :       ret.x = sc.x + K * (z - (H * sc.x));
+     216      595838 :       ret.P = (P_t::Identity() - (K * H)) * sc.P;
+     217     1191740 :       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       16429 :     virtual statecov_t predict(const statecov_t& sc, const u_t& u, const Q_t& Q, double dt) const override
+     317             :     {
+     318       16429 :       statecov_t ret;
+     319       16429 :       A_t A = m_generateA(dt);
+     320       16429 :       B_t B = m_generateB(dt);
+     321       16429 :       ret.x = Base_class::state_predict(A, sc.x, B, u);
+     322       16429 :       ret.P = Base_class::covariance_predict(A, sc.P, Q, dt);
+     323       32858 :       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..13a9dce68e --- /dev/null +++ b/mrs_lib/include/mrs_lib/msg_extractor.h.func-sort-c.html @@ -0,0 +1,256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/msg_extractor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - msg_extractor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:339933.3 %
Date:2024-04-09 22:03:33Functions: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&)263
mrs_lib::getHeading(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)263
mrs_lib::getPosition(mrs_msgs::Reference_<std::allocator<void> > const&)263
mrs_lib::getPosition(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)263
mrs_lib::getPosition(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)263
mrs_lib::getPose(nav_msgs::Odometry_<std::allocator<void> > const&)1501
mrs_lib::getHeading(geometry_msgs::Pose_<std::allocator<void> > const&)190146
mrs_lib::getHeading(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)190147
mrs_lib::getHeading(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)190147
mrs_lib::getHeading(nav_msgs::Odometry_<std::allocator<void> > const&)190147
mrs_lib::getPosition(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)190148
mrs_lib::getPosition(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)190149
mrs_lib::getPosition(geometry_msgs::Pose_<std::allocator<void> > const&)190149
mrs_lib::getPosition(nav_msgs::Odometry_<std::allocator<void> > const&)190149
mrs_lib::getXYZ(geometry_msgs::Point_<std::allocator<void> > const&)190674
+
+
+ + + +
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..4b49998c7d --- /dev/null +++ b/mrs_lib/include/mrs_lib/msg_extractor.h.func.html @@ -0,0 +1,256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/msg_extractor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - msg_extractor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:339933.3 %
Date:2024-04-09 22:03:33Functions: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&)190147
mrs_lib::getHeading(geometry_msgs::Pose_<std::allocator<void> > const&)190146
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&)190147
mrs_lib::getHeading(mrs_msgs::Reference_<std::allocator<void> > const&)263
mrs_lib::getHeading(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)0
mrs_lib::getHeading(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)263
mrs_lib::getHeading(nav_msgs::Odometry_<std::allocator<void> > const&)190147
mrs_lib::getPosition(geometry_msgs::PoseWithCovariance_<std::allocator<void> > const&)190149
mrs_lib::getPosition(geometry_msgs::Pose_<std::allocator<void> > const&)190149
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&)190148
mrs_lib::getPosition(mrs_msgs::Reference_<std::allocator<void> > const&)263
mrs_lib::getPosition(mrs_msgs::TrackerCommand_<std::allocator<void> > const&)263
mrs_lib::getPosition(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)263
mrs_lib::getPosition(nav_msgs::Odometry_<std::allocator<void> > const&)190149
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&)190674
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&)1501
+
+
+ + + +
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..93c5002268 --- /dev/null +++ b/mrs_lib/include/mrs_lib/msg_extractor.h.gcov.html @@ -0,0 +1,775 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/msg_extractor.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - msg_extractor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:339933.3 %
Date:2024-04-09 22:03:33Functions: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      190674 : std::tuple<double, double, double> getXYZ(const geometry_msgs::Point& data) {
+      30             : 
+      31      190674 :   double x = data.x;
+      32      190674 :   double y = data.y;
+      33      190674 :   double z = data.z;
+      34             : 
+      35      381348 :   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      190149 : std::tuple<double, double, double> getPosition(const geometry_msgs::Pose& data) {
+      96             : 
+      97      190149 :   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      190146 : double getHeading(const geometry_msgs::Pose& data) {
+     124             : 
+     125      190146 :   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      190149 : std::tuple<double, double, double> getPosition(const geometry_msgs::PoseWithCovariance& data) {
+     184             : 
+     185      190149 :   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      190147 : double getHeading(const geometry_msgs::PoseWithCovariance& data) {
+     212             : 
+     213      190147 :   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      190149 : std::tuple<double, double, double> getPosition(const nav_msgs::Odometry& data) {
+     336             : 
+     337      190149 :   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      190148 : std::tuple<double, double, double> getPosition(const nav_msgs::OdometryConstPtr& data) {
+     348             : 
+     349      190148 :   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      190147 : double getHeading(const nav_msgs::Odometry& data) {
+     392             : 
+     393      190147 :   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      190147 : double getHeading(const nav_msgs::OdometryConstPtr& data) {
+     404             : 
+     405      190147 :   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        1501 : geometry_msgs::Pose getPose(const nav_msgs::Odometry& data) {
+     448             : 
+     449        1501 :   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         263 : std::tuple<double, double, double> getPosition(const mrs_msgs::TrackerCommand& data) {
+     480             : 
+     481         263 :   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         263 : std::tuple<double, double, double> getPosition(const mrs_msgs::Reference& data) {
+     579             : 
+     580         263 :   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         263 : double getHeading(const mrs_msgs::Reference& data) {
+     607             : 
+     608         263 :   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         263 : std::tuple<double, double, double> getPosition(const mrs_msgs::ReferenceStamped& data) {
+     639             : 
+     640         263 :   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         263 : double getHeading(const mrs_msgs::ReferenceStamped& data) {
+     667             : 
+     668         263 :   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..ab61ddd65e --- /dev/null +++ b/mrs_lib/include/mrs_lib/mutex.h.func-sort-c.html @@ -0,0 +1,400 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/mutex.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - mutex.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1111100.0 %
Date:2024-04-09 22:03:33Functions:668082.5 %
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_msgs::ConstraintsOverrideRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::ConstraintsOverrideRequest_<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_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&)82
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> >&)91
auto mrs_lib::set_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig, mrs_uav_controllers::mpc_controllerConfig&)164
auto mrs_lib::set_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig, mrs_uav_controllers::se3_controllerConfig&)164
mrs_msgs::ObstacleSectors_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ObstacleSectors_<std::allocator<void> > >(std::mutex&, mrs_msgs::ObstacleSectors_<std::allocator<void> >&)439
mrs_msgs::VelocityReference_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::VelocityReference_<std::allocator<void> > >(std::mutex&, mrs_msgs::VelocityReference_<std::allocator<void> >&)497
mrs_msgs::ReferenceStamped_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(std::mutex&, mrs_msgs::ReferenceStamped_<std::allocator<void> >&)894
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> >&)1786
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&)4359
int mrs_lib::get_mutexed<int>(std::mutex&, int&)6957
std::tuple<int, double> mrs_lib::get_mutexed<int, double>(std::mutex&, int&, double&)12195
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&)12225
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&)12536
std::optional<double> mrs_lib::get_mutexed<std::optional<double> >(std::mutex&, std::optional<double>&)15041
std::tuple<double, double> mrs_lib::get_mutexed<double, double>(std::mutex&, double&, double&)15307
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&)16124
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&)20950
std::tuple<double, double, double> mrs_lib::get_mutexed<double, double, double>(std::mutex&, double&, double&, double&)25592
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> >&)30895
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&)31129
mrs_uav_controllers::se3_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig&)55446
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&)59539
mrs_msgs::MpcPredictionFullState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::MpcPredictionFullState_<std::allocator<void> > >(std::mutex&, mrs_msgs::MpcPredictionFullState_<std::allocator<void> >&)59539
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&)61200
std::tuple<bool, double, double> mrs_lib::get_mutexed<bool, double, double>(std::mutex&, bool&, double&, double&)65275
mrs_uav_trackers::mpc_trackerConfig mrs_lib::get_mutexed<mrs_uav_trackers::mpc_trackerConfig>(std::mutex&, mrs_uav_trackers::mpc_trackerConfig&)65275
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> >&)72994
auto mrs_lib::set_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput, mrs_uav_managers::Controller::ControlOutput&)86776
mrs_uav_controllers::mpc_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig&)110859
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> >&)122028
mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&)134493
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>&)155337
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&)166662
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&)167839
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&)173520
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> >&)178102
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>&)179116
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>&)192089
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> >&)194157
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>&)210219
mrs_msgs::DynamicsConstraints_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraints_<std::allocator<void> >&)224941
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&)250147
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>&)265574
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&)276907
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&)291440
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&)313640
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&)334144
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&)334191
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> >&)335705
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> >&)457624
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&)507057
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> > >&)584472
double const mrs_lib::get_mutexed<double const>(std::mutex&, double const&)602654
mrs_uav_managers::Controller::ControlOutput mrs_lib::get_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput&)667750
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&)678054
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&)679026
ros::Time mrs_lib::get_mutexed<ros::Time>(std::mutex&, ros::Time&)811588
unsigned int mrs_lib::get_mutexed<unsigned int>(std::mutex&, unsigned int&)826986
mrs_msgs::UavState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::UavState_<std::allocator<void> > >(std::mutex&, mrs_msgs::UavState_<std::allocator<void> >&)858384
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> >&)876325
double mrs_lib::get_mutexed<double>(std::mutex&, double&)885285
auto mrs_lib::set_mutexed<double>(std::mutex&, double, double&)951895
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&)2107865
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&)2232136
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&)2325825
+
+
+ + + +
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..80b4050aea --- /dev/null +++ b/mrs_lib/include/mrs_lib/mutex.h.func.html @@ -0,0 +1,400 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/mutex.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - mutex.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1111100.0 %
Date:2024-04-09 22:03:33Functions:668082.5 %
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&)59539
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>&)192089
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&)276907
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>&)179116
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&)12225
std::tuple<bool, double, double> mrs_lib::get_mutexed<bool, double, double>(std::mutex&, bool&, double&, double&)65275
std::tuple<double, double> mrs_lib::get_mutexed<double, double>(std::mutex&, double&, double&)15307
std::tuple<double, double, double> mrs_lib::get_mutexed<double, double, double>(std::mutex&, double&, double&, double&)25592
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&)20950
std::tuple<int, double> mrs_lib::get_mutexed<int, double>(std::mutex&, int&, double&)12195
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&)2232136
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&)166662
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&)507057
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&)173520
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&)250147
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&)167839
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&)313640
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&)2107865
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&)2325825
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&)16124
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&)291440
double const mrs_lib::get_mutexed<double const>(std::mutex&, double const&)602654
mrs_uav_managers::Controller::ControlOutput mrs_lib::get_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput&)667750
mrs_uav_trackers::mpc_trackerConfig mrs_lib::get_mutexed<mrs_uav_trackers::mpc_trackerConfig>(std::mutex&, mrs_uav_trackers::mpc_trackerConfig&)65275
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&)61200
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&)31129
mrs_uav_controllers::mpc_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig&)110859
mrs_uav_controllers::se3_controllerConfig mrs_lib::get_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig&)55446
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&)811588
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>&)265574
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>&)155337
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>&)210219
mrs_msgs::ObstacleSectors_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ObstacleSectors_<std::allocator<void> > >(std::mutex&, mrs_msgs::ObstacleSectors_<std::allocator<void> >&)439
mrs_msgs::ReferenceStamped_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ReferenceStamped_<std::allocator<void> > >(std::mutex&, mrs_msgs::ReferenceStamped_<std::allocator<void> >&)894
mrs_msgs::VelocityReference_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::VelocityReference_<std::allocator<void> > >(std::mutex&, mrs_msgs::VelocityReference_<std::allocator<void> >&)497
mrs_msgs::DynamicsConstraints_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraints_<std::allocator<void> >&)224941
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> >&)59539
mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> >&)0
mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > >(std::mutex&, mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&)134493
mrs_msgs::UavState_<std::allocator<void> > mrs_lib::get_mutexed<mrs_msgs::UavState_<std::allocator<void> > >(std::mutex&, mrs_msgs::UavState_<std::allocator<void> >&)858384
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&)679026
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&)334191
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> >&)72994
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> >&)194157
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> > >&)584472
std::optional<double> mrs_lib::get_mutexed<std::optional<double> >(std::mutex&, std::optional<double>&)15041
double mrs_lib::get_mutexed<double>(std::mutex&, double&)885285
int mrs_lib::get_mutexed<int>(std::mutex&, int&)6957
unsigned int mrs_lib::get_mutexed<unsigned int>(std::mutex&, unsigned int&)826986
auto mrs_lib::set_mutexed<mrs_uav_managers::Controller::ControlOutput>(std::mutex&, mrs_uav_managers::Controller::ControlOutput, mrs_uav_managers::Controller::ControlOutput&)86776
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&)12536
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&)4359
auto mrs_lib::set_mutexed<mrs_uav_controllers::mpc_controllerConfig>(std::mutex&, mrs_uav_controllers::mpc_controllerConfig, mrs_uav_controllers::mpc_controllerConfig&)164
auto mrs_lib::set_mutexed<mrs_uav_controllers::se3_controllerConfig>(std::mutex&, mrs_uav_controllers::se3_controllerConfig, mrs_uav_controllers::se3_controllerConfig&)164
auto mrs_lib::set_mutexed<mrs_uav_trajectory_generation::drsConfig>(std::mutex&, mrs_uav_trajectory_generation::drsConfig, mrs_uav_trajectory_generation::drsConfig&)82
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> >&)122028
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> >&)1786
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> >&)457624
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> >&)91
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> >&)876325
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> >&)335705
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&)678054
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&)334144
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> >&)30895
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> >&)178102
auto mrs_lib::set_mutexed<double>(std::mutex&, double, double&)951895
+
+
+ + + +
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..19f04fe3ca --- /dev/null +++ b/mrs_lib/include/mrs_lib/mutex.h.gcov.html @@ -0,0 +1,260 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/mutex.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - mutex.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1111100.0 %
Date:2024-04-09 22:03:33Functions:668082.5 %
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      859195 : std::tuple<Args...> get_mutexed(std::mutex& mut, Args&... args) {
+      49             : 
+      50     1718391 :   std::scoped_lock lock(mut);
+      51             : 
+      52      859196 :   std::tuple result = std::tuple(args...);
+      53             : 
+      54     1718392 :   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    16467621 : T get_mutexed(std::mutex& mut, T& arg) {
+      69             : 
+      70    27099242 :   std::scoped_lock lock(mut);
+      71             : 
+      72    32933339 :   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     4070730 : auto set_mutexed(std::mutex& mut, const T what, T& where) {
+     117             : 
+     118     7166813 :   std::scoped_lock lock(mut);
+     119             : 
+     120     4063415 :   where = what;
+     121             : 
+     122     8136707 :   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..5e7203a962 --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.func-sort-c.html @@ -0,0 +1,392 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/param_loader.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - param_loader.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:27130090.3 %
Date:2024-04-09 22:03:33Functions:7878100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ParamLoader::loadParam2(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&)1
XmlRpc::XmlRpcValue mrs_lib::ParamLoader::loadParam2<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&)1
mrs_lib::ParamLoader::print_warning(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
void mrs_lib::ParamLoader::loadMatrixArray<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > >&)1
void mrs_lib::ParamLoader::loadMatrixArray<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > >&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&)1
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixKnown2<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)1
Eigen::Matrix<float, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixKnown2<float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > > const&, int, int)1
bool mrs_lib::ParamLoader::loadMatrixStatic<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0>&)1
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float, Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&, Eigen::MatrixBase<Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> > const&)1
bool mrs_lib::ParamLoader::loadMatrixDynamic<double, Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> > const&, int, int)1
bool mrs_lib::ParamLoader::loadMatrixDynamic<double, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, int, int)1
Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0> mrs_lib::ParamLoader::loadMatrixStatic2<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic2<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1
Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> mrs_lib::ParamLoader::loadMatrixStatic2<3, 3, float, Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::Product<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> >, 0> > const&)1
bool mrs_lib::ParamLoader::loadParamReusable<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&)1
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic2<double, Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::MatrixBase<Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> const, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> > const> > const&, int, int)1
std::pair<Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0>, bool> mrs_lib::ParamLoader::loadMatrixStatic_internal<0, 0, double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<double, 0, 0, ((Eigen::StorageOptions)0)|((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)1) : ((((0)==(1))&&((0)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 0, 0> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)1
bool mrs_lib::ParamLoader::loadMatrixKnown<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1>&, int, int)2
bool mrs_lib::ParamLoader::loadMatrixKnown<float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, -1, -1, 0, -1, -1> > > const&, int, int)2
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&)2
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float, Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_identity_op<float>, Eigen::Matrix<float, 3, 3, 0, 3, 3> > > const&)2
mrs_lib::ParamLoader::loadParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, XmlRpc::XmlRpcValue const&)2
bool mrs_lib::ParamLoader::loadParam<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, XmlRpc::XmlRpcValue const&)2
mrs_lib::ParamLoader::printValue(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3
bool mrs_lib::ParamLoader::loadMatrixStatic<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>&)3
mrs_lib::ParamLoader::resetLoadedSuccessfully()3
mrs_lib::ParamLoader::loadParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
bool mrs_lib::ParamLoader::loadParam<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&)3
std::pair<Eigen::Matrix<float, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixKnown_internal<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)4
mrs_lib::ParamLoader::printValue_recursive(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue&, unsigned int)5
std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > mrs_lib::ParamLoader::loadMatrixArray_internal<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<Eigen::Matrix<double, -1, -1, 0, -1, -1>, std::allocator<Eigen::Matrix<double, -1, -1, 0, -1, -1> > > const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)5
mrs_lib::ParamLoader::resolved(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)5
std::pair<Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3>, bool> mrs_lib::ParamLoader::loadMatrixStatic_internal<3, 3, float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, 3, 3, ((Eigen::StorageOptions)0)|((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)1) : ((((3)==(1))&&((3)!=(1)))?((Eigen::StorageOptions)0) : ((Eigen::StorageOptions)0))), 3, 3> const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)6
std::pair<XmlRpc::XmlRpcValue, bool> mrs_lib::ParamLoader::load<XmlRpc::XmlRpcValue>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, XmlRpc::XmlRpcValue const&, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t)6
mrs_lib::ParamLoader::printError(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)7
void mrs_lib::ParamLoader::printValue<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&)8
std::pair<Eigen::Matrix<float, -1, -1, 0, -1, -1>, bool> mrs_lib::ParamLoader::loadMatrixX<float>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Matrix<float, -1, -1, 0, -1, -1> const&, int, int, mrs_lib::ParamLoader::optional_t, mrs_lib::ParamLoader::unique_t, mrs_lib::ParamLoader::swap_t, bool)10
mrs_lib::ParamLoader::resetUniques()20
bool mrs_lib::ParamLoader::loadParam<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)39
Eigen::Matrix<double, -1, -1, 0, -1, -1> mrs_lib::ParamLoader::loadMatrixDynamic2<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int, int)63
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)65
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&)82
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)82
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)82
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)149
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)328
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)328
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&)410
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)565
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&)569
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&, double const&)656
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&)902
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)902
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> >&)902
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, std::basic_string_view<char, std::char_traits<char> >)1382
mrs_lib::ParamLoader::setPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1597
mrs_lib::ParamLoader::addYamlFileFromParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2196
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&)2861
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)2862
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> > > >&)2862
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, bool, std::basic_string_view<char, std::char_traits<char> >)3176
mrs_lib::ParamLoader::loadedSuccessfully()3554
void mrs_lib::ParamLoader::printValue<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int const&)4143
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)4143
bool mrs_lib::ParamLoader::loadParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&)4143
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&, bool const&)4850
mrs_lib::ParamLoader::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)9056
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&)9659
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> >&)13377
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&)13827
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)13827
void mrs_lib::ParamLoader::printValue<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)14508
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)14509
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&)32827
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)33483
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)33483
mrs_lib::ParamLoader::check_duplicit_loading(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)70310
+
+
+ + + +
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..09dde5e92a --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.func.html @@ -0,0 +1,392 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/param_loader.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - param_loader.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:27130090.3 %
Date:2024-04-09 22:03:33Functions: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&)410
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&)2861
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&)13827
void mrs_lib::ParamLoader::printValue<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool const&)14508
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&)569
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&)902
void mrs_lib::ParamLoader::printValue<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double const&)33483
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&)4143
mrs_lib::ParamLoader::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)9056
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)565
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)328
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&)82
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)65
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)82
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)63
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()3554
mrs_lib::ParamLoader::addYamlFileFromParam(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)2196
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&)70310
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)328
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)82
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)149
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)13827
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)2862
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)902
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)14509
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)33483
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)4143
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> >&)13377
bool mrs_lib::ParamLoader::loadParam<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)39
bool mrs_lib::ParamLoader::loadParam<std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > >(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >&)2862
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> >&)902
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&)9659
bool mrs_lib::ParamLoader::loadParam<bool>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool&, bool const&)4850
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&)32827
bool mrs_lib::ParamLoader::loadParam<double>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double&, double const&)656
bool mrs_lib::ParamLoader::loadParam<int>(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, int&)4143
mrs_lib::ParamLoader::setPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1597
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, std::basic_string_view<char, std::char_traits<char> >)1382
mrs_lib::ParamLoader::ParamLoader(ros::NodeHandle const&, bool, std::basic_string_view<char, std::char_traits<char> >)3176
+
+
+ + + +
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..d2914a9226 --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.gcov.html @@ -0,0 +1,1452 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/param_loader.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - param_loader.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:27130090.3 %
Date:2024-04-09 22:03:33Functions:7878100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : // clang: MatousFormat
+       2             : /**  \file
+       3             :      \brief Defines ParamLoader - a convenience class for loading static ROS parameters.
+       4             :      \author Matouš Vrba - vrbamato@fel.cvut.cz
+       5             :  */
+       6             : 
+       7             : #ifndef PARAM_LOADER_H
+       8             : #define PARAM_LOADER_H
+       9             : 
+      10             : #include <ros/ros.h>
+      11             : #include <string>
+      12             : #include <map>
+      13             : #include <unordered_set>
+      14             : #include <iostream>
+      15             : #include <Eigen/Dense>
+      16             : #include <std_msgs/ColorRGBA.h>
+      17             : #include <mrs_lib/param_provider.h>
+      18             : 
+      19             : namespace mrs_lib
+      20             : {
+      21             : 
+      22             : /*** ParamLoader CLASS //{ **/
+      23             : 
+      24             : /**
+      25             : * \brief Convenience class for loading parameters from rosparam server.
+      26             : *
+      27             : * The parameters can be loaded as compulsory. If a compulsory parameter is not found
+      28             : * on the rosparam server (e.g. because it is missing in the launchfile or the yaml config file),
+      29             : * an internal flag is set to false, indicating that the parameter loading procedure failed.
+      30             : * This flag can be checked using the loaded_successfully() method after all parameters were
+      31             : * attempted to be loaded (see usage example usage below).
+      32             : *
+      33             : * The loaded parameter names and corresponding values are printed to stdout by default
+      34             : * for user convenience. Special cases such as loading of Eigen matrices or loading
+      35             : * of std::vectors of various values are also provided.
+      36             : *
+      37             : * To load parameters into the `rosparam` server, use a launchfile prefferably.
+      38             : * See documentation of ROS launchfiles here: http://wiki.ros.org/roslaunch/XML.
+      39             : * Specifically, the `param` XML tag is used for loading parameters directly from the launchfile: http://wiki.ros.org/roslaunch/XML/param,
+      40             : * and the `rosparam` XML tag tag is used for loading parameters from a `yaml` file: http://wiki.ros.org/roslaunch/XML/rosparam.
+      41             : *
+      42             : */
+      43             : class ParamLoader
+      44             : {
+      45             : 
+      46             : private:
+      47             :   enum unique_t
+      48             :   {
+      49             :     UNIQUE = true,
+      50             :     REUSABLE = false
+      51             :   };
+      52             :   enum optional_t
+      53             :   {
+      54             :     OPTIONAL = true,
+      55             :     COMPULSORY = false
+      56             :   };
+      57             :   enum swap_t
+      58             :   {
+      59             :     SWAP = true,
+      60             :     NO_SWAP = false
+      61             :   };
+      62             : 
+      63             :   template <typename T>
+      64             :   using MatrixX = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
+      65             : 
+      66             : private:
+      67             :   bool m_load_successful, m_print_values;
+      68             :   std::string m_node_name;
+      69             :   std::string m_prefix;
+      70             :   const ros::NodeHandle& m_nh;
+      71             :   mrs_lib::ParamProvider m_pp;
+      72             :   std::unordered_set<std::string> m_loaded_params;
+      73             : 
+      74             :   /* printing helper functions //{ */
+      75             :   /* printError and print_warning functions //{*/
+      76           7 :   void printError(const std::string& str)
+      77             :   {
+      78           7 :     if (m_node_name.empty())
+      79           7 :       ROS_ERROR_STREAM(str);
+      80             :     else
+      81           0 :       ROS_ERROR_STREAM("[" << m_node_name << "]: " << str);
+      82           7 :   }
+      83           1 :   void print_warning(const std::string& str)
+      84             :   {
+      85           1 :     if (m_node_name.empty())
+      86           1 :       ROS_WARN_STREAM(str);
+      87             :     else
+      88           0 :       ROS_WARN_STREAM("[" << m_node_name << "]: " << str);
+      89           1 :   }
+      90             :   //}
+      91             : 
+      92             :   /* printValue function and overloads //{ */
+      93             : 
+      94             :   template <typename T>
+      95       65961 :   void printValue(const std::string& name, const T& value)
+      96             :   {
+      97       65961 :     if (m_node_name.empty())
+      98       36960 :       std::cout << "\t" << name << ":\t" << value << std::endl;
+      99             :     else
+     100       29001 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << name << "':\t" << value);
+     101       65962 :   }
+     102             : 
+     103             :   template <typename T>
+     104        3763 :   void printValue(const std::string& name, const std::vector<T>& value)
+     105             :   {
+     106        7526 :     std::stringstream strstr;
+     107        3763 :     if (m_node_name.empty())
+     108        1830 :       strstr << "\t";
+     109        3763 :     strstr << name << ":\t";
+     110        3763 :     size_t it = 0;
+     111       13175 :     for (const auto& elem : value)
+     112             :     {
+     113        9412 :       strstr << elem;
+     114        9412 :       if (it < value.size() - 1)
+     115        6064 :         strstr << ", ";
+     116        9412 :       it++;
+     117             :     }
+     118        3763 :     if (m_node_name.empty())
+     119        1830 :       std::cout << strstr.str() << std::endl;
+     120             :     else
+     121        1933 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << strstr.str());
+     122        3763 :   }
+     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         577 :   void printValue(const std::string& name, const MatrixX<T>& value)
+     147             :   {
+     148        1154 :     std::stringstream strstr;
+     149             :     /* const Eigen::IOFormat fmt(4, 0, ", ", "\n", "\t\t[", "]"); */
+     150             :     /* strstr << value.format(fmt); */
+     151        1731 :     const Eigen::IOFormat fmt;
+     152         577 :     strstr << value.format(fmt);
+     153         577 :     if (m_node_name.empty())
+     154          85 :       std::cout << "\t" << name << ":\t" << std::endl << strstr.str() << std::endl;
+     155             :     else
+     156         492 :       ROS_INFO_STREAM("[" << m_node_name << "]: parameter '" << name << "':" << std::endl << strstr.str());
+     157         577 :   }
+     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       70310 :   bool check_duplicit_loading(const std::string& name)
+     216             :   {
+     217       70310 :     if (m_loaded_params.count(name))
+     218             :     {
+     219           2 :       printError(std::string("Tried to load parameter ") + name + std::string(" twice"));
+     220           2 :       m_load_successful = false;
+     221           2 :       return true;
+     222             :     } else
+     223             :     {
+     224       70308 :       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         575 :   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        1150 :     const std::string name_prefixed = m_prefix + name;
+     235        1150 :     MatrixX<T> loaded = default_value;
+     236         575 :     bool used_rosparam_value = false;
+     237             :     // first, check if the user already tried to load this parameter
+     238         575 :     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         575 :     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         575 :     const bool expect_zero_matrix = rows == 0;
+     250         575 :     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         575 :     bool cur_load_successful = true;
+     261         575 :     bool check_size_exact = true;
+     262         575 :     if (cols <= 0)  // this means that the cols dimension is dynamic or a zero matrix is expected
+     263         147 :       check_size_exact = false;
+     264             : 
+     265        1150 :     std::vector<T> tmp_vec;
+     266             :     // try to load the parameter
+     267         575 :     const bool success = m_pp.getParam(name_prefixed, tmp_vec);
+     268             :     // check if the loaded vector has correct length
+     269         575 :     bool correct_size = (int)tmp_vec.size() == rows * cols;
+     270         575 :     if (!check_size_exact && !expect_zero_matrix)
+     271         146 :       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         575 :     if (success && correct_size)
+     274             :     {
+     275             :       // if successfully loaded, everything is in order
+     276             :       // transform the vector to the matrix
+     277         484 :       if (cols <= 0 && rows > 0)
+     278         146 :         cols = tmp_vec.size() / rows;
+     279         484 :       if (swap)
+     280          63 :         std::swap(rows, cols);
+     281         484 :       loaded = Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>, Eigen::Unaligned>(tmp_vec.data(), rows, cols);
+     282         484 :       used_rosparam_value = true;
+     283             :     } else
+     284             :     {
+     285          91 :       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          91 :       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         575 :     if (cur_load_successful)
+     309             :     {
+     310         572 :       if (m_print_values && printValues)
+     311         567 :         printValue(name_prefixed, loaded);
+     312         572 :       m_loaded_params.insert(name_prefixed);
+     313             :     } else
+     314             :     {
+     315           3 :       m_load_successful = false;
+     316             :     }
+     317             :     // finally, return the resulting value
+     318         575 :     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          89 :   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         267 :     const auto [dynamic, loaded_ok] = loadMatrixX(name, MatrixX<T>(default_value), rows, cols, optional, unique, NO_SWAP);
+     327         178 :     return {dynamic, loaded_ok};
+     328             :   }
+     329             :   //}
+     330             : 
+     331             :   /* loadMatrixKnown_internal helper function for loading EigenXd matrices with known dimensions //{ */
+     332             :   template <typename T>
+     333         332 :   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         664 :     MatrixX<T> loaded = default_value;
+     336             :     // first, check that at least one dimension is set
+     337         332 :     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         332 :     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         149 :   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         298 :     MatrixX<T> loaded = default_value;
+     353             : 
+     354             :     // next, check that at least one dimension is set
+     355         149 :     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         149 :     swap_t swap = NO_SWAP;
+     363         149 :     if (rows <= 0)
+     364             :     {
+     365          63 :       std::swap(rows, cols);
+     366          63 :       swap = SWAP;
+     367             :     }
+     368         149 :     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       69732 :   std::pair<T, bool> load(const std::string& name, const T& default_value, optional_t optional = OPTIONAL, unique_t unique = UNIQUE)
+     453             :   {
+     454      139464 :     const std::string name_prefixed = m_prefix + name;
+     455       87329 :     T loaded = default_value;
+     456       69732 :     if (unique && check_duplicit_loading(name_prefixed))
+     457           2 :       return {loaded, false};
+     458             : 
+     459       69730 :     bool cur_load_successful = true;
+     460             :     // try to load the parameter
+     461       69730 :     const bool success = m_pp.getParam(name_prefixed, loaded);
+     462       69729 :     if (!success)
+     463             :     {
+     464             :       // if it was not loaded, set the default value
+     465        1254 :       loaded = default_value;
+     466        1254 :       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       69728 :     if (cur_load_successful)
+     475             :     {
+     476             :       // everything is fine and just print the name_prefixed and value if required
+     477       69726 :       if (m_print_values)
+     478       69727 :         printValue(name_prefixed, loaded);
+     479             :       // mark the param name_prefixed as successfully loaded
+     480       69727 :       m_loaded_params.insert(name_prefixed);
+     481             :     } else
+     482             :     {
+     483           2 :       m_load_successful = false;
+     484             :     }
+     485             :     // finally, return the resulting value
+     486       69730 :     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        3176 :   ParamLoader(const ros::NodeHandle& nh, bool printValues = true, std::string_view node_name = std::string())
+     499        3176 :       : m_load_successful(true),
+     500             :         m_print_values(printValues),
+     501             :         m_node_name(node_name),
+     502             :         m_nh(nh),
+     503        3176 :         m_pp(nh, m_node_name)
+     504             :   {
+     505             :     /* std::cout << "Initialized1 ParamLoader for node " << node_name << std::endl; */
+     506        3176 :   }
+     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        1382 :   ParamLoader(const ros::NodeHandle& nh, std::string_view node_name)
+     516        1382 :       : ParamLoader(nh, true, node_name)
+     517             :   {
+     518             :     /* std::cout << "Initialized2 ParamLoader for node " << node_name << std::endl; */
+     519        1382 :   }
+     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        1597 :   void setPrefix(const std::string& prefix)
+     542             :   {
+     543        1597 :     m_prefix = prefix;
+     544        1597 :   }
+     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        9056 :   bool addYamlFile(const std::string& filepath)
+     571             :   {
+     572        9056 :     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        2196 :   bool addYamlFileFromParam(const std::string& param_name)
+     585             :   {
+     586        4392 :     std::string filepath;
+     587        2196 :     if (!loadParam(param_name, filepath))
+     588           0 :       return false;
+     589        2196 :     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        3554 :   bool loadedSuccessfully()
+     600             :   {
+     601        3554 :     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        5547 :   bool loadParam(const std::string& name, T& out_value, const T& default_value)
+     640             :   {
+     641        5547 :     const auto [ret, success] = load<T>(name, default_value, OPTIONAL, UNIQUE);
+     642        5547 :     out_value = ret;
+     643        5588 :     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         411 :   T loadParam2(const std::string& name, const T& default_value)
+     658             :   {
+     659         822 :     const auto loaded = load<T>(name, default_value, OPTIONAL, UNIQUE);
+     660         822 :     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       63773 :   bool loadParam(const std::string& name, T& out_value)
+     714             :   {
+     715       80917 :     const auto [ret, success] = load<T>(name, T(), COMPULSORY, UNIQUE);
+     716       63773 :     out_value = ret;
+     717       80917 :     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          85 :   bool loadMatrixStatic(const std::string& name, Eigen::Matrix<T, rows, cols>& mat, const Eigen::MatrixBase<Derived>& default_value)
+    1001             :   {
+    1002          85 :     const auto [ret, loaded_ok] = loadMatrixStatic_internal<rows, cols, T>(name, Eigen::Matrix<T, rows, cols>(default_value), OPTIONAL, UNIQUE);
+    1003          85 :     mat = ret;
+    1004          85 :     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         330 :   bool loadMatrixKnown(const std::string& name, MatrixX<T>& mat, int rows, int cols)
+    1076             :   {
+    1077         660 :     const auto [ret, loaded_ok] = loadMatrixKnown_internal(name, MatrixX<T>(), rows, cols, COMPULSORY, UNIQUE);
+    1078         330 :     mat = ret;
+    1079         660 :     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          65 :   bool loadMatrixDynamic(const std::string& name, MatrixX<T>& mat, int rows, int cols)
+    1171             :   {
+    1172         130 :     const auto [ret, loaded_ok] = loadMatrixDynamic_internal(name, MatrixX<T>(), rows, cols, COMPULSORY, UNIQUE);
+    1173          65 :     mat = ret;
+    1174         130 :     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          84 :   bool loadMatrixDynamic(const std::string& name, MatrixX<T>& mat, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
+    1195             :   {
+    1196         168 :     const auto [ret, loaded_ok] = loadMatrixDynamic_internal(name, MatrixX<T>(default_value), rows, cols, OPTIONAL, UNIQUE);
+    1197          84 :     mat = ret;
+    1198         168 :     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          63 :   MatrixX<T> loadMatrixDynamic2(const std::string& name, int rows, int cols)
+    1217             :   {
+    1218          63 :     MatrixX<T> ret;
+    1219          63 :     loadMatrixDynamic(name, ret, rows, cols);
+    1220          63 :     return ret;
+    1221             :   }
+    1222             : 
+    1223             :   /*!
+    1224             :     * \brief Specialized method for loading compulsory dynamic Eigen matrix parameters.
+    1225             :     *
+    1226             :     * This variant assumes that the only one of the matrix dimensions are known, the other is selected based on the loaded value.
+    1227             :     * If the dimensions of the loaded matrix do not match the specified number of rows and columns, the loading process is unsuccessful (loaded_successfully() will return false).
+    1228             :     * If the parameter with the specified name is not found on the rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+    1229             :     * the default value is used.
+    1230             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1231             :     *
+    1232             :     * \param name          Name of the parameter in the rosparam server.
+    1233             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1234             :     * \param rows          Expected number of rows of the matrix (negative value indicates that the number of rows is to be deduced from the specified number of columns and the size of the loaded array).
+    1235             :     * \param cols          Expected number of columns of the matrix (negative value indicates that the number of columns is to be deduced from the specified number of rows and the size of the loaded array).
+    1236             :     * \return              The loaded parameter value.
+    1237             :     */
+    1238             :   template <typename T, typename Derived>
+    1239           1 :   MatrixX<T> loadMatrixDynamic2(const std::string& name, const Eigen::MatrixBase<Derived>& default_value, int rows, int cols)
+    1240             :   {
+    1241           1 :     MatrixX<T> ret;
+    1242           1 :     loadMatrixDynamic(name, ret, default_value, rows, cols);
+    1243           1 :     return ret;
+    1244             :   }
+    1245             : 
+    1246             :   //}
+    1247             : 
+    1248             :   // loadMatrixArray function for loading of an array of MatrixX<T> with known dimensions //{
+    1249             :   /*!
+    1250             :     * \brief Specialized method for loading compulsory parameters, interpreted as an array of dynamic Eigen matrices.
+    1251             :     *
+    1252             :     * The number of rows and columns of the matrices to be loaded is specified in the \c rosparam parameter. Specifically, the \c name/rows value specifies the
+    1253             :     * number of rows, which must be common to all the loaded matrices (i.e. it is one integer >= 0), and the \c name/cols value specifies the number of columns of
+    1254             :     * each matrix (i.e. it is an array of integers > 0). The \c name/data array contains the values of the elements of the matrices and it must have length
+    1255             :     * \f$ r\sum_i c_i \f$, where \f$ r \f$ is the common number of rows and \f$ c_i \f$ is the number of columns of the \f$ i \f$-th matrix.
+    1256             :     * A typical structure of a \c yaml file, specifying the
+    1257             :     * matrix array to be loaded using this method, is
+    1258             :     *
+    1259             :     * \code{.yaml}
+    1260             :     *
+    1261             :     * matrix_array:
+    1262             :     *   rows: 3
+    1263             :     *   cols: [1, 2]
+    1264             :     *   data: [-5.0, 0.0, 23.0,
+    1265             :     *          -5.0, 0.0, 12.0,
+    1266             :     *           2.0,   4.0,  7.0]
+    1267             :     *
+    1268             :     * \endcode
+    1269             :     *
+    1270             :     * which will be loaded as a \c std::vector, containing one \f$ 3\times 1 \f$ matrix and one \f$ 3\times 2 \f$ matrix.
+    1271             :     *
+    1272             :     * If the dimensions of the loaded matrices do not match the specified number of rows and columns, the loading process is unsuccessful (loaded_successfully() will return false).
+    1273             :     * If the parameter with the specified name is not found on the \c rosparam server (e.g. because it is not specified in the launchfile or yaml config file),
+    1274             :     * the loading process is unsuccessful.
+    1275             :     * Using this method, the parameter can only be loaded once using the same ParamLoader instance without error.
+    1276             :     *
+    1277             :     * \param name  Name of the parameter in the rosparam server.
+    1278             :     * \param mat   Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1279             :     *
+    1280             :     */
+    1281             :   template <typename T>
+    1282           1 :   void loadMatrixArray(const std::string& name, std::vector<MatrixX<T>>& mat)
+    1283             :   {
+    1284           1 :     mat = loadMatrixArray2<double>(name);
+    1285           1 :   }
+    1286             : 
+    1287             :   /*!
+    1288             :     * \brief Specialized method for loading compulsory parameters, interpreted as an array of dynamic Eigen matrices.
+    1289             :     *
+    1290             :     * This overload of the loadMatrixArray() method takes a default value for the parameter, which is used in case a \c rosparam with the specified name is not
+    1291             :     * found in the \c rosparam server, instead of causing an unsuccessful load. This makes specifying the parameter value in the \c rosparam server optional.
+    1292             :     *
+    1293             :     * \param name           Name of the parameter in the rosparam server.
+    1294             :     * \param mat            Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    1295             :     * \param default_value  The default value to be used in case the parameter is not found on the \c rosparam server.
+    1296             :     *
+    1297             :     */
+    1298             :   template <typename T>
+    1299           1 :   void loadMatrixArray(const std::string& name, std::vector<MatrixX<T>>& mat, const std::vector<MatrixX<T>>& default_value)
+    1300             :   {
+    1301           1 :     mat = loadMatrixArray2(name, default_value);
+    1302           1 :   }
+    1303             : 
+    1304             :   /*!
+    1305             :     * \brief Specialized method for loading compulsory parameters, interpreted as an array of dynamic Eigen matrices.
+    1306             :     *
+    1307             :     * This method works in the same way as the loadMatrixArray() method for compulsory parameters, except that the loaded
+    1308             :     * parameter is returned and not stored in the reference parameter.
+    1309             :     *
+    1310             :     * \param name           Name of the parameter in the rosparam server.
+    1311             :     * \returns              The loaded parameter or a default constructed object of the respective type.
+    1312             :     *
+    1313             :     */
+    1314             :   template <typename T = double>
+    1315           3 :   std::vector<MatrixX<T>> loadMatrixArray2(const std::string& name)
+    1316             :   {
+    1317           3 :     return loadMatrixArray_internal(name, std::vector<MatrixX<T>>(), COMPULSORY, UNIQUE);
+    1318             :   }
+    1319             : 
+    1320             :   /*!
+    1321             :     * \brief Specialized method for loading compulsory parameters, interpreted as an array of dynamic Eigen matrices.
+    1322             :     *
+    1323             :     * This method works in the same way as the loadMatrixArray() method for optional parameters, except that the loaded
+    1324             :     * parameter is returned and not stored in the reference parameter.
+    1325             :     *
+    1326             :     * \param name           Name of the parameter in the rosparam server.
+    1327             :     * \param default_value  The default value to be used in case the parameter is not found on the \c rosparam server.
+    1328             :     * \returns              The loaded parameter or the default value.
+    1329             :     *
+    1330             :     */
+    1331             :   template <typename T>
+    1332           2 :   std::vector<MatrixX<T>> loadMatrixArray2(const std::string& name, const std::vector<MatrixX<T>>& default_value)
+    1333             :   {
+    1334           2 :     return loadMatrixArray_internal(name, default_value, OPTIONAL, UNIQUE);
+    1335             :   }
+    1336             :   //}
+    1337             : 
+    1338             :   //}
+    1339             : 
+    1340             : };
+    1341             : //}
+    1342             : 
+    1343             :   /*!
+    1344             :     * \brief An overload for loading ros::Duration.
+    1345             :     *
+    1346             :     * The duration will be loaded as a \p double, representing a number of seconds, and then converted to ros::Duration.
+    1347             :     *
+    1348             :     * \param name          Name of the parameter in the rosparam server.
+    1349             :     * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    1350             :     * \return              The loaded parameter value.
+    1351             :     */
+    1352             :   template <>
+    1353             :   ros::Duration ParamLoader::loadParam2<ros::Duration>(const std::string& name, const ros::Duration& default_value);
+    1354             : 
+    1355             :   /*!
+    1356             :     * \brief An overload for loading ros::Duration.
+    1357             :     *
+    1358             :     * The duration will be loaded as a \p double, representing a number of seconds, and then converted to ros::Duration.
+    1359             :     *
+    1360             :     * \param name          Name of the parameter in the rosparam server.
+    1361             :     * \return              The loaded parameter value.
+    1362             :     */
+    1363             :   template <>
+    1364             :   ros::Duration ParamLoader::loadParam2<ros::Duration>(const std::string& name);
+    1365             : 
+    1366             : }  // namespace mrs_lib
+    1367             : 
+    1368             : #endif  // PARAM_LOADER_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/param_loader.h.gcov.overview.html b/mrs_lib/include/mrs_lib/param_loader.h.gcov.overview.html new file mode 100644 index 0000000000..06a945b67d --- /dev/null +++ b/mrs_lib/include/mrs_lib/param_loader.h.gcov.overview.html @@ -0,0 +1,362 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/param_loader.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()0
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::PublisherHandler()1
mrs_lib::PublisherHandler_impl<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~PublisherHandler_impl()1
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::PublisherHandler()2
mrs_lib::PublisherHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~PublisherHandler()2
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler()2
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~PublisherHandler_impl()2
mrs_lib::PublisherHandler_impl<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler_impl()2
mrs_lib::PublisherHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~PublisherHandler()4
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler()25
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler_impl()25
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler()50
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<std_msgs::String_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()83
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler_impl()85
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::PublisherHandler()165
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::PublisherHandler()168
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::PublisherHandler()168
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler_impl()168
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler_impl()231
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler()246
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler()246
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler()246
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler()246
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler()246
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler()246
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()246
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()246
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler()248
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler()253
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::PublisherHandler()316
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler_impl()316
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::PublisherHandler()328
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::PublisherHandler()328
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler_impl()328
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler_impl()328
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler()336
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler_impl()399
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler()463
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::~PublisherHandler()463
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler_impl()471
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::PublisherHandler()492
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler_impl()492
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler()501
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::PublisherHandler()549
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::PublisherHandler()608
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler_impl()608
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler()632
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler()656
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler()656
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler()780
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::PublisherHandler()857
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler()984
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler()1216
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler()1328
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler()133835
+
+
+ + + +
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..de39345fbb --- /dev/null +++ b/mrs_lib/include/mrs_lib/publisher_handler.h.func.html @@ -0,0 +1,556 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/publisher_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - publisher_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions:11811999.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::PublisherHandler()328
mrs_lib::PublisherHandler<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler()656
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::PublisherHandler()168
mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler()253
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::PublisherHandler()492
mrs_lib::PublisherHandler<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler()984
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::PublisherHandler()857
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler()1328
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::PublisherHandler()316
mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler()632
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()82
mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler()246
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler()246
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler()246
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::PublisherHandler()608
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler()1216
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::PublisherHandler()549
mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler()780
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::PublisherHandler()165
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler()248
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::PublisherHandler()463
mrs_lib::PublisherHandler<mrs_msgs::EstimatorDiagnostics_<std::allocator<void> > >::~PublisherHandler()463
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >::~PublisherHandler()246
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler()246
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler()246
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()246
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler()246
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::PublisherHandler()168
mrs_lib::PublisherHandler<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler()336
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::PublisherHandler()501
mrs_lib::PublisherHandler<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler()133835
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::PublisherHandler()25
mrs_lib::PublisherHandler<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler()50
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<std_msgs::Int64_<std::allocator<void> > >::~PublisherHandler()2
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::PublisherHandler()82
mrs_lib::PublisherHandler<std_msgs::String_<std::allocator<void> > >::~PublisherHandler()164
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::PublisherHandler()328
mrs_lib::PublisherHandler<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler()656
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseArray_<std::allocator<void> > >::~PublisherHandler_impl()328
mrs_lib::PublisherHandler_impl<geometry_msgs::PoseStamped_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~PublisherHandler_impl()85
mrs_lib::PublisherHandler_impl<visualization_msgs::MarkerArray_<std::allocator<void> > >::~PublisherHandler_impl()492
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlError_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~PublisherHandler_impl()471
mrs_lib::PublisherHandler_impl<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~PublisherHandler_impl()316
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()82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >::~PublisherHandler_impl()608
mrs_lib::PublisherHandler_impl<mrs_msgs::Float64ArrayStamped_<std::allocator<void> > >::~PublisherHandler_impl()231
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()83
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()82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::Path_<std::allocator<void> > >::~PublisherHandler_impl()82
mrs_lib::PublisherHandler_impl<mrs_msgs::UavState_<std::allocator<void> > >::~PublisherHandler_impl()168
mrs_lib::PublisherHandler_impl<nav_msgs::Odometry_<std::allocator<void> > >::~PublisherHandler_impl()399
mrs_lib::PublisherHandler_impl<std_msgs::Bool_<std::allocator<void> > >::~PublisherHandler_impl()25
mrs_lib::PublisherHandler_impl<std_msgs::Empty_<std::allocator<void> > >::~PublisherHandler_impl()82
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()82
mrs_lib::PublisherHandler_impl<std_msgs::Float64_<std::allocator<void> > >::~PublisherHandler_impl()328
+
+
+ + + +
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..c62e39098e --- /dev/null +++ b/mrs_lib/include/mrs_lib/publisher_handler.h.gcov.html @@ -0,0 +1,256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/publisher_handler.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - publisher_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions:11811999.2 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /**  \file
+       2             :      \brief Defines PublisherHandler and related convenience classes for upgrading the ROS publisher
+       3             :      \author Tomas Baca - tomas.baca@fel.cvut.cz
+       4             :  */
+       5             : #ifndef PUBLISHER_HANDLER_H
+       6             : #define PUBLISHER_HANDLER_H
+       7             : 
+       8             : #include <ros/ros.h>
+       9             : #include <ros/package.h>
+      10             : 
+      11             : #include <atomic>
+      12             : #include <string>
+      13             : #include <mutex>
+      14             : 
+      15             : namespace mrs_lib
+      16             : {
+      17             : 
+      18             : /* class PublisherHandler_impl //{ */
+      19             : 
+      20             : /**
+      21             :  * @brief implementation of the publisher handler
+      22             :  */
+      23             : template <class TopicType>
+      24             : class PublisherHandler_impl {
+      25             : 
+      26             : public:
+      27             :   /**
+      28             :    * @brief default constructor
+      29             :    */
+      30             :   PublisherHandler_impl(void);
+      31             : 
+      32             :   /**
+      33             :    * @brief default destructor
+      34             :    */
+      35        5507 :   ~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        7595 :   PublisherHandler(void){};
+     102             : 
+     103             :   /**
+     104             :    * @brief generic destructor
+     105             :    */
+     106      146037 :   ~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         194 :   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..49e22f5c17 --- /dev/null +++ b/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func-sort-c.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/quadratic_throttle_model.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - quadratic_throttle_model.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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)70030
mrs_lib::quadratic_throttle_model::throttleToForce(mrs_lib::quadratic_throttle_model::MotorParams_t, double)105617
+
+
+ + + +
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..4d18ada824 --- /dev/null +++ b/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.func.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/quadratic_throttle_model.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - quadratic_throttle_model.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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)70030
mrs_lib::quadratic_throttle_model::throttleToForce(mrs_lib::quadratic_throttle_model::MotorParams_t, double)105617
+
+
+ + + +
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..9db1175db2 --- /dev/null +++ b/mrs_lib/include/mrs_lib/quadratic_throttle_model.h.gcov.html @@ -0,0 +1,117 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/quadratic_throttle_model.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - quadratic_throttle_model.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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      105617 : double inline throttleToForce(const MotorParams_t motor_params, const double throttle) {
+      20             : 
+      21      105617 :   return motor_params.n_motors * pow((throttle - motor_params.B) / motor_params.A, 2);
+      22             : }
+      23             : 
+      24       70030 : double inline forceToThrottle(const MotorParams_t motor_params, const double force) {
+      25             : 
+      26       70030 :   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..8d83cd319f --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.func-sort-c.html @@ -0,0 +1,256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/repredictor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - repredictor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10011190.1 %
Date:2024-04-09 22:03:33Functions: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&)105
std::enable_if<!(false), void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::addInputChangeWithNoise<false>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&)200
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&, Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&)200
std::enable_if<true, void>::type mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, true>::addInputChangeWithNoise<true>(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, Eigen::Matrix<double, 2, 2, 0, 2, 2> const&, ros::Time const&, std::shared_ptr<mrs_lib::varstepLKF<2, 1, 1> > const&)202
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&)292
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&)292
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&)584
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&)584
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)620
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&)2059
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&)5976
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&)15345
+
+
+ + + +
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..15971f1532 --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.func.html @@ -0,0 +1,256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/repredictor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - repredictor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10011190.1 %
Date:2024-04-09 22:03:33Functions: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&)15345
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&)5976
mrs_lib::Repredictor<mrs_lib::varstepLKF<2, 1, 1>, false>::info_t::info_t(ros::Time const&)620
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&)2059
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&)292
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&)584
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&)105
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&)292
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&)202
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&)584
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..5bf8489ede --- /dev/null +++ b/mrs_lib/include/mrs_lib/repredictor.h.gcov.html @@ -0,0 +1,637 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/repredictor.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - repredictor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10011190.1 %
Date:2024-04-09 22:03:33Functions: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       15243 :       do
+      81             :       {
+      82       15345 :         cur_sc.stamp = hist_it->stamp;
+      83             :         // do the correction if current history point is a measurement
+      84       15345 :         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       15345 :         ros::Time next_stamp = to_stamp;
+      90       15345 :         if ((hist_it + 1) != std::end(m_history) && (hist_it + 1)->stamp <= to_stamp)
+      91       15243 :           next_stamp = (hist_it + 1)->stamp;
+      92             : 
+      93             :         // do the prediction
+      94       15345 :         cur_sc = predictFrom(cur_sc, *hist_it, cur_stamp, next_stamp);
+      95       15345 :         cur_stamp = next_stamp;
+      96             : 
+      97             :         // increment the history pointer
+      98       15345 :         hist_it++;
+      99       15345 :       } 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         584 :     std::enable_if_t<check, statecov_t> predictTo(const ros::Time& to_stamp)
+     118             :     {
+     119         584 :       assert(!m_history.empty());
+     120         584 :       const auto& info = m_history.front();
+     121         584 :       auto sc = predictFrom(m_sc, info, info.stamp, to_stamp);
+     122         584 :       sc.stamp = to_stamp;
+     123         584 :       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        6076 :         for (auto it = added + 1; it != std::end(m_history) && it->is_measurement; it++)
+     151        5876 :           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         202 :     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         202 :       if (m_history.empty())
+     170           2 :         m_history.push_back({stamp});
+     171         202 :       m_history.front().u = u;
+     172         202 :       m_history.front().Q = Q;
+     173         202 :       m_history.front().stamp = stamp;
+     174         202 :       m_history.front().predict_model = model;
+     175         202 :     }
+     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         105 :     std::enable_if_t<check> addInputChange(const u_t& u, [[maybe_unused]] const ros::Time& stamp, const ModelPtr& model = nullptr)
+     221             :     {
+     222         105 :       if (m_history.empty())
+     223           0 :         m_history.push_back({stamp});
+     224         105 :       m_history.front().u = u;
+     225         105 :       m_history.front().stamp = stamp;
+     226         105 :       m_history.front().predict_model = model;
+     227         105 :     }
+     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         292 :     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         292 :       if (m_history.empty())
+     325           0 :         m_history.push_back({stamp});
+     326         292 :       auto& info = m_history.front();
+     327         292 :       const ros::Time to_stamp = stamp > info.stamp ? stamp : info.stamp;
+     328         292 :       const auto sc = predictTo(to_stamp);
+     329         292 :       info.z = z;
+     330         292 :       info.R = R;
+     331         292 :       info.stamp = to_stamp;
+     332         292 :       info.is_measurement = true;
+     333         292 :       info.meas_id = meas_id;
+     334         292 :       info.correct_model = model;
+     335         292 :       m_sc = correctFrom(sc, info);
+     336         292 :     }
+     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         622 :       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        5976 :       void updateUsing(const info_t& info)
+     431             :       {
+     432        5976 :         u = info.u;
+     433        5976 :         Q = info.Q;
+     434        5976 :         predict_model = info.predict_model;
+     435        5976 :       };
+     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        2059 :     static bool earlier(const info_t& ob1, const info_t& ob2)
+     526             :     {
+     527        2059 :       return ob1.stamp < ob2.stamp;
+     528             :     }
+     529             :     //}
+     530             : 
+     531             :     /* predictFrom() method //{ */
+     532       15929 :     statecov_t predictFrom(const statecov_t& sc, const info_t& inpt, const ros::Time& from_stamp, const ros::Time& to_stamp)
+     533             :     {
+     534       31858 :       const auto model = inpt.predict_model == nullptr ? m_default_model : inpt.predict_model;
+     535       15929 :       const auto dt = (to_stamp - from_stamp).toSec();
+     536       31858 :       return model->predict(sc, inpt.u, inpt.Q, dt);
+     537             :     }
+     538             :     //}
+     539             : 
+     540             :     /* correctFrom() method //{ */
+     541        5442 :     statecov_t correctFrom(const statecov_t& sc, const info_t& meas)
+     542             :     {
+     543        5442 :       assert(meas.is_measurement);
+     544       10884 :       const auto model = meas.correct_model == nullptr ? m_default_model : meas.correct_model;
+     545        5442 :       auto sc_tmp = sc;
+     546       10884 :       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..9f0bb91e82 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-04-09 22:03:33Functions: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..8420eee32f --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index-detail-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-04-09 22:03:33Functions: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..2f603c2828 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index-detail.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-04-09 22:03:33Functions: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..a3c4ccb6a7 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-04-09 22:03:33Functions: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..16752c7301 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-04-09 22:03:33Functions: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..0dc1b16078 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:080.0 %
Date:2024-04-09 22:03:33Functions: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..99455c887c --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/polygon.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/polygon.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - polygon.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:060.0 %
Date:2024-04-09 22:03:33Functions: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..1a63771bb0 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/polygon.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/polygon.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - polygon.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:060.0 %
Date:2024-04-09 22:03:33Functions: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..c3d4e21b29 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/polygon.h.gcov.html @@ -0,0 +1,136 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/polygon.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - polygon.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:060.0 %
Date:2024-04-09 22:03:33Functions: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..0f0f7b9fac --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func-sort-c.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/safety_zone.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - safety_zone.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:020.0 %
Date:2024-04-09 22:03:33Functions: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..cb3e87ffe2 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.func.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/safety_zone.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - safety_zone.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:020.0 %
Date:2024-04-09 22:03:33Functions: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..b4380220a5 --- /dev/null +++ b/mrs_lib/include/mrs_lib/safety_zone/safety_zone.h.gcov.html @@ -0,0 +1,121 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/safety_zone/safety_zone.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib/safety_zone - safety_zone.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:020.0 %
Date:2024-04-09 22:03:33Functions: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..690450a8ac --- /dev/null +++ b/mrs_lib/include/mrs_lib/scope_timer.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/scope_timer.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - scope_timer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2633.3 %
Date:2024-04-09 22:03:33Functions: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&)4790134
+
+
+ + + +
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..901fba9e6a --- /dev/null +++ b/mrs_lib/include/mrs_lib/scope_timer.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/scope_timer.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - scope_timer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2633.3 %
Date:2024-04-09 22:03:33Functions: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&)4790134
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..50d4aa6817 --- /dev/null +++ b/mrs_lib/include/mrs_lib/scope_timer.h.gcov.html @@ -0,0 +1,248 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/scope_timer.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - scope_timer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2633.3 %
Date:2024-04-09 22:03:33Functions: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     4790134 :     time_point(const std::string& label) : ros_time(ros::Time::now()), chrono_time(std::chrono::steady_clock::now()), label(label) {
+      93     4789916 :     }
+      94             : 
+      95             :     time_point(const std::string& label, const ros::Time& ros_time) : ros_time(ros_time), label(label) {
+      96             :       // helper types to make the code slightly more readable
+      97             :       using float_seconds   = std::chrono::duration<float>;
+      98             :       using chrono_duration = std::chrono::steady_clock::duration;
+      99             :       // prepare ros and chrono current times to minimize their difference
+     100             :       const auto ros_now    = ros::Time::now();
+     101             :       const auto chrono_now = std::chrono::steady_clock::now();
+     102             :       // calculate how old is ros_time
+     103             :       const auto ros_dt = ros_now - ros_time;
+     104             :       // cast ros_dt to chrono type
+     105             :       const auto chrono_dt = std::chrono::duration_cast<chrono_duration>(float_seconds(ros_dt.toSec()));
+     106             :       // calculate ros_time in chrono time and set it to chrono_time
+     107             :       chrono_time = chrono_now - chrono_dt;
+     108             :     }
+     109             : 
+     110             :     ros::Time   ros_time;
+     111             :     chrono_tp   chrono_time;
+     112             :     std::string label;
+     113             :   };
+     114             :   //}
+     115             : 
+     116             : public:
+     117             :   /**
+     118             :    * @brief The basic constructor with a user-defined label of the timer, throttled period and file logger.
+     119             :    */
+     120             :   ScopeTimer(const std::string& label, const ros::Duration& throttle_period = ros::Duration(0), const bool enable = true,
+     121             :              const std::shared_ptr<ScopeTimerLogger> scope_timer_logger = nullptr);
+     122             : 
+     123             :   /**
+     124             :    * @brief The basic constructor with a user-defined label of the timer and a pre-start time, which will also be measured.
+     125             :    */
+     126             :   ScopeTimer(const std::string& label, const time_point& tp0, const ros::Duration& throttle_period = ros::Duration(0), const bool enable = true,
+     127             :              const std::shared_ptr<ScopeTimerLogger> scope_timer_logger = nullptr);
+     128             : 
+     129             :   /**
+     130             :    * @brief The basic constructor with a user-defined label of the timer and file logger.
+     131             :    */
+     132             :   ScopeTimer(const std::string& label, const std::shared_ptr<ScopeTimerLogger> scope_timer_logger, const bool enable = true);
+     133             : 
+     134             :   /**
+     135             :    * @brief Checkpoint, prints the time passed until the point this function is called.
+     136             :    */
+     137             :   void checkpoint(const std::string& label);
+     138             : 
+     139             :   /**
+     140             :    * @brief Getter for scope timer lifetime
+     141             :    *
+     142             :    * @return lifetime as floating point milliseconds
+     143             :    */
+     144             :   float getLifetime();
+     145             : 
+     146             :   /**
+     147             :    * @brief The basic destructor which prints out or logs the scope times, if enabled.
+     148             :    */
+     149             :   ~ScopeTimer();
+     150             : 
+     151             : private:
+     152             :   std::string             _timer_label_;
+     153             :   bool                    _enable_print_or_log;
+     154             :   ros::Duration           _throttle_period_;
+     155             :   std::vector<time_point> checkpoints;
+     156             : 
+     157             :   std::shared_ptr<ScopeTimerLogger> _logger_ = nullptr;
+     158             : 
+     159             :   static std::unordered_map<std::string, ros::Time> last_print_times;
+     160             : };
+     161             : 
+     162             : }  // namespace mrs_lib
+     163             : 
+     164             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/scope_timer.h.gcov.overview.html b/mrs_lib/include/mrs_lib/scope_timer.h.gcov.overview.html new file mode 100644 index 0000000000..82f8580772 --- /dev/null +++ b/mrs_lib/include/mrs_lib/scope_timer.h.gcov.overview.html @@ -0,0 +1,61 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/scope_timer.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler()82
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler()82
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler_impl()82
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::~ServiceClientHandler_impl()82
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler()83
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler_impl()83
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler()164
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::~ServiceClientHandler()164
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler()164
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::~ServiceClientHandler_impl()164
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler()166
mrs_lib::ServiceClientHandler<mrs_msgs::String>::~ServiceClientHandler()328
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler()492
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::~ServiceClientHandler_impl()492
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::~ServiceClientHandler()984
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler()994
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::~ServiceClientHandler_impl()994
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::~ServiceClientHandler()1906
+
+
+ + + +
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..d86698791e --- /dev/null +++ b/mrs_lib/include/mrs_lib/service_client_handler.h.func.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/service_client_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - service_client_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:33100.0 %
Date:2024-04-09 22:03:33Functions:1818100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::ServiceClientHandler()83
mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler()166
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::ServiceClientHandler()82
mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler()164
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::ServiceClientHandler()82
mrs_lib::ServiceClientHandler<mrs_msgs::Vec1>::~ServiceClientHandler()164
mrs_lib::ServiceClientHandler<mrs_msgs::String>::ServiceClientHandler()164
mrs_lib::ServiceClientHandler<mrs_msgs::String>::~ServiceClientHandler()328
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::ServiceClientHandler()492
mrs_lib::ServiceClientHandler<std_srvs::SetBool>::~ServiceClientHandler()984
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::ServiceClientHandler()994
mrs_lib::ServiceClientHandler<std_srvs::Trigger>::~ServiceClientHandler()1906
mrs_lib::ServiceClientHandler_impl<mrs_msgs::ReferenceStampedSrv>::~ServiceClientHandler_impl()83
mrs_lib::ServiceClientHandler_impl<mrs_msgs::DynamicsConstraintsSrv>::~ServiceClientHandler_impl()82
mrs_lib::ServiceClientHandler_impl<mrs_msgs::Vec1>::~ServiceClientHandler_impl()82
mrs_lib::ServiceClientHandler_impl<mrs_msgs::String>::~ServiceClientHandler_impl()164
mrs_lib::ServiceClientHandler_impl<std_srvs::SetBool>::~ServiceClientHandler_impl()492
mrs_lib::ServiceClientHandler_impl<std_srvs::Trigger>::~ServiceClientHandler_impl()994
+
+
+ + + +
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..05cf7b84fa --- /dev/null +++ b/mrs_lib/include/mrs_lib/service_client_handler.h.gcov.html @@ -0,0 +1,327 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/service_client_handler.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - service_client_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:33100.0 %
Date:2024-04-09 22:03:33Functions:1818100.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        1897 :   ~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        1897 :   ServiceClientHandler(void){};
+     135             : 
+     136             :   /**
+     137             :    * @brief generic destructor
+     138             :    */
+     139        3712 :   ~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..d86ef5763b --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.func-sort-c.html @@ -0,0 +1,3044 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/subscribe_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - subscribe_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:444989.8 %
Date:2024-04-09 22:03:33Functions:38774152.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> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::ProcTfToWorld<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >&&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::TfMappingOrigin>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::TfMappingOrigin>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::control_manager::ControlManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<2>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::ProcTfToWorld<1>::*)(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>), mrs_uav_state_estimators::ProcTfToWorld<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::TfMappingOrigin::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::TfMappingOrigin*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_managers::control_manager::ControlManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::control_manager::ControlManager*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::function<void (std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)> const&, void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<Tester>(mrs_lib::SubscribeHandlerOptions const&, void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*)1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*)1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::topicName[abi:cxx11]() const1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::function<void (std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&)> const&, void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))::{lambda()#1}::operator()() const1
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler<void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (*)(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&), void (*)(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>))::{lambda()#1}::operator()() const1
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (Tester::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), Tester*)::{lambda()#1}::operator()() const1
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)2
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<mrs_uav_state_estimators::Correction<1>>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)2
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const2
mrs_lib::SubscribeHandler<mrs_msgs::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::ControllerDiagnostics_<std::allocator<void> > >::getMsg()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<geometry_msgs::PointStamped_<std::allocator<void> > >::lastMsgTime() const5
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::hasMsg() const6
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::start()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)> const&)9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >&&)9
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::start()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >&&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::mpc_tracker::MpcTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::start()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >&&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::mpc_tracker::MpcTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)::{lambda()#1}::operator()() const10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)::{lambda()#1}::operator()() const10
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::hasMsg() const15
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()25
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&)25
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&)25
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >&&)25
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&)25
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*)25
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*)25
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::start()25
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&)25
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()25
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&)25
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >&&)25
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::start()25
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&)25
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler()25
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*)25
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*)25
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >&&)25
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()() const25
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()() const25
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()() const25
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()() const25
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()() const25
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()50
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()50
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::getMsg()72
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::hasMsg() const72
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const78
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const78
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*)79
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*)79
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()() const79
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>*)81
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>*)81
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>*)81
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>*)81
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()() const81
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()() const81
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*)82
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*)82
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*)82
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*)82
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&)82
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*)82
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*)82
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&)82
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >&&)82
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*)82
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*)82
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&)82
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*)82
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*)82
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*)82
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*)82
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*)82
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*)82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler()82
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*)82
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*)82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler()82
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*)82
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*)82
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler()82
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*)82
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*)82
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler()82
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*)82
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*)82
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler()82
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*)82
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*)82
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler()82
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*)82
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*)82
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >&&)82
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*)82
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*)82
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*)82
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*)82
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*)82
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*)82
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*)82
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*)82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::topicName[abi:cxx11]() const82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::hasMsg() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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&)83
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()() const83
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&)85
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>*)85
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*)85
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>*)85
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*)85
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*)85
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*)85
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*)85
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*)85
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&)85
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::start()85
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&)85
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler()85
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&)85
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >&&)85
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()() const85
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()() const85
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()() const85
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()() const85
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()() const85
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()() const85
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()() const85
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::start()86
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*)86
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*)86
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()() const86
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const97
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const108
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>*)128
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>*)128
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()() const128
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::start()153
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&)153
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >&&)153
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::lastMsgTime() const162
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::start()164
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&)164
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler()164
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >&&)164
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::start()164
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&)164
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler()164
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&)164
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >&&)164
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::start()164
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&)164
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler()164
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&)164
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >&&)164
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()() const164
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()() const164
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::start()167
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&)167
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >&&)167
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler()168
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~SubscribeHandler()170
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::start()189
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&)189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler()189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >&&)189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler()189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >&&)189
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&)234
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()() const234
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::getMsg()237
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&)246
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::start()246
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&)246
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler()246
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >&&)246
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()() const246
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::start()252
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&)252
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >&&)252
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::start()259
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&)259
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >&&)259
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::start()271
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&)271
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&)271
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()() const271
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::lastMsgTime() const301
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler()304
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::~SubscribeHandler()304
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::SubscribeHandler()304
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::~SubscribeHandler()304
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler()305
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::start()313
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&)313
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler()313
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&)313
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >&&)313
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()() const313
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::start()324
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&)324
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >&&)324
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler()327
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::~SubscribeHandler()328
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::start()328
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&)328
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler()328
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >&&)328
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~SubscribeHandler()328
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::~SubscribeHandler()328
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler()329
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler()329
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::~SubscribeHandler()335
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::start()353
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&)353
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler()353
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&)353
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >&&)353
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::start()353
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&)353
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()353
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&)353
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >&&)353
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()() const353
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()() const353
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::~SubscribeHandler()354
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::~SubscribeHandler()378
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::SubscribeHandler()386
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::~SubscribeHandler()387
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::~SubscribeHandler()395
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::getMsg()421
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::~SubscribeHandler()460
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler()474
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::~SubscribeHandler()482
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::start()486
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&)486
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >&&)486
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::~SubscribeHandler()492
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::getMsg()549
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::hasMsg() const549
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::getMsg()611
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~SubscribeHandler()626
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler()638
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::~SubscribeHandler()651
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~SubscribeHandler()656
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~SubscribeHandler()706
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()706
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::~SubscribeHandler()733
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::~SubscribeHandler()890
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler()1095
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~SubscribeHandler()1581
mrs_lib::SubscribeHandlerOptions::SubscribeHandlerOptions()2097
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::hasMsg() const2357
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::getMsg()3238
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::getMsg()3808
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::getMsg()9517
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::hasMsg() const9517
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::getMsg()13507
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::hasMsg() const16411
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::hasMsg() const16975
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::hasMsg() const17005
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::hasMsg() const17183
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::hasMsg() const17213
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::getMsg()19280
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::getMsg()32190
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::lastMsgTime() const32190
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::getMsg()38346
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::hasMsg() const46336
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::hasMsg() const59504
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::getMsg()61975
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::hasMsg() const63716
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::hasMsg() const133881
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::getMsg()151728
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::hasMsg() const151728
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::getMsg()166914
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::lastMsgTime() const179269
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::getMsg()193349
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::hasMsg() const209357
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::getMsg()340865
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::hasMsg() const344768
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::lastMsgTime() const367637
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::hasMsg() const463654
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::newMsg() const469992
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::getMsg()644444
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::getMsg()760079
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::hasMsg() const825900
+
+
+ + + +
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..c724132862 --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.func.html @@ -0,0 +1,3044 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/subscribe_handler.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - subscribe_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:444989.8 %
Date:2024-04-09 22:03:33Functions:38774152.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> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::start()324
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&)324
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::SubscribeHandler()327
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*)82
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*)82
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*)79
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>*)81
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*)82
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*)82
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*)79
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>*)81
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::~SubscribeHandler()651
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >&&)324
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::start()25
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::getMsg()9517
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&)25
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::SubscribeHandler()329
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&)25
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()354
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >&&)25
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::start()164
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&)164
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::SubscribeHandler()164
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&)82
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*)82
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*)82
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::~SubscribeHandler()328
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >&&)164
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::start()153
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::getMsg()13507
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&)153
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::SubscribeHandler()329
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&)25
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>*)128
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>*)128
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()482
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >&&)153
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)> const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::SubscribeHandler()304
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()304
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >&&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::stop()5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::start()86
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::getMsg()3808
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&)82
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::SubscribeHandler()305
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>*)81
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>*)81
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::~SubscribeHandler()387
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::start()259
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::getMsg()166914
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&)259
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::SubscribeHandler()474
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&)85
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>*)85
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*)85
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>*)85
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*)85
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::~SubscribeHandler()733
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >&&)259
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::start()486
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::getMsg()644444
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&)486
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::SubscribeHandler()1095
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&)234
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*)82
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*)85
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*)85
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*)82
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*)85
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*)85
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::~SubscribeHandler()1581
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >&&)486
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::start()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::SubscribeHandler()304
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::~SubscribeHandler()304
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::start()189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::getMsg()237
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&)189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::SubscribeHandler()189
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&)82
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*)82
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*)25
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*)82
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*)25
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::~SubscribeHandler()378
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >&&)189
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::start()167
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::getMsg()151728
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&)167
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::SubscribeHandler()168
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&)85
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*)82
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*)82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::~SubscribeHandler()335
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >&&)167
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::start()313
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::getMsg()340865
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&)313
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::SubscribeHandler()313
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&)313
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::~SubscribeHandler()626
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >&&)313
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::start()328
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::getMsg()38346
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&)328
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::SubscribeHandler()328
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&)246
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*)82
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*)82
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::~SubscribeHandler()656
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >&&)328
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::start()164
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::getMsg()611
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&)164
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::SubscribeHandler()164
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&)164
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::~SubscribeHandler()328
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >&&)164
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::start()85
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&)85
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::SubscribeHandler()85
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&)85
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::~SubscribeHandler()170
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >&&)85
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::start()82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::getMsg()549
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::SubscribeHandler()82
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*)82
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*)82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::start()82
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::getMsg()421
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::SubscribeHandler()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::start()10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >&&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::mpc_tracker::MpcTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::~SubscribeHandler()20
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::SubscribeHandler()82
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*)82
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*)82
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::start()271
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::getMsg()19280
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&)271
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::SubscribeHandler()189
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&)271
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::~SubscribeHandler()460
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >&&)189
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::SubscribeHandler()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::SubscribeHandler()82
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*)82
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*)82
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::SubscribeHandler()82
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*)82
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*)82
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::start()82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::getMsg()4
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::SubscribeHandler()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::start()353
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::getMsg()61975
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&)353
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::SubscribeHandler()353
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&)353
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::~SubscribeHandler()706
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >&&)353
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::start()10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::getMsg()0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >&&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler(mrs_lib::SubscribeHandlerOptions const&, std::function<void (boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)> const&)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::SubscribeHandler<mrs_uav_trackers::mpc_tracker::MpcTracker>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_trackers::mpc_tracker::MpcTracker::*)(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>), mrs_uav_trackers::mpc_tracker::MpcTracker*)10
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()20
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::start()25
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&)25
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()25
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&)25
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()50
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >&&)25
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::start()82
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::getMsg()0
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::start()25
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&)25
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::SubscribeHandler()25
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*)25
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*)25
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()50
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >&&)25
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::SubscribeHandler()82
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*)82
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*)82
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::start()353
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::getMsg()760079
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&)353
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()353
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&)353
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()706
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >&&)353
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::start()82
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::getMsg()0
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::SubscribeHandler()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::start()82
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&)82
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::SubscribeHandler()82
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*)82
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*)82
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::~SubscribeHandler()164
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >&&)82
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::start()9
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::getMsg()72
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()386
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()395
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >&&)9
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::start()246
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::getMsg()3238
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&)246
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::SubscribeHandler()246
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*)82
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*)82
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*)82
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*)82
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*)82
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*)82
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::~SubscribeHandler()492
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >&&)246
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::start()252
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::getMsg()193349
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&)252
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler()638
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&)83
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*)82
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*)86
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<mrs_uav_managers::uav_manager::UavManager>(mrs_lib::SubscribeHandlerOptions const&, void (mrs_uav_managers::uav_manager::UavManager::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_managers::uav_manager::UavManager*)82
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*)86
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()890
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >&&)252
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::waitForNew(ros::WallDuration const&)0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::stop()0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::start()164
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::getMsg()32190
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&)164
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler()164
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&)164
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::~SubscribeHandler()328
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::operator=(mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >&&)164
mrs_lib::SubscribeHandlerOptions::SubscribeHandlerOptions()2097
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::hasMsg() const9517
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Imu_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Joy_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::hasMsg() const16975
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<sensor_msgs::Range_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::lastMsgTime() const5
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::hasMsg() const16411
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PointStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::hasMsg() const344768
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped_<std::allocator<void> > >::hasMsg() const825900
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]() const108
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<geometry_msgs::PoseWithCovarianceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::lastMsgTime() const162
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::hasMsg() const17183
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude_<std::allocator<void> > >::hasMsg() const151728
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() const367637
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::newMsg() const469992
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::hasMsg() const59504
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::hasMsg() const15
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimatorOutput_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels_<std::allocator<void> > >::hasMsg() const549
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() const301
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::hasMsg() const2357
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities_<std::allocator<void> > >::hasMsg() const17213
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]() const82
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::hasMsg() const6
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::SpeedTrackerCommand_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::hasMsg() const82
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControllerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::hasMsg() const63716
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics_<std::allocator<void> > >::hasMsg() const17005
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() const78
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics_<std::allocator<void> > >::hasMsg() const133881
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]() const97
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::lastMsgTime() const78
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::hasMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::Path_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::RtkGps_<std::allocator<void> > >::hasMsg() const72
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() const179269
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::hasMsg() const209357
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<mrs_msgs::UavState_<std::allocator<void> > >::topicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::lastMsgTime() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::hasMsg() const463654
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::newMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::peekMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::usedMsg() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::topicName[abi:cxx11]() const1
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::lastMsgTime() const32190
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::subscribedTopicName[abi:cxx11]() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::hasMsg() const46336
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()() const82
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()() const82
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()() const79
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()() const81
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()() const25
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()() const82
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()() const82
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()() const25
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()() const128
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()() const81
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()() const85
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()() const85
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()() const85
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()() const234
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()() const82
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()() const85
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()() const85
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()() const82
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()() const82
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()() const25
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()() const85
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()() const82
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()() const313
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()() const246
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()() const82
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()() const164
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()() const85
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()() const82
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()() const82
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()() const82
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()() const271
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()() const82
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()() const82
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()() const82
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()() const82
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()() const353
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()() const25
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()() const82
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()() const25
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()() const82
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()() const353
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()() const82
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()() const82
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()() const82
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()() const82
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()() const82
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()() const83
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()() const82
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()() const86
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<1>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<1>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<nav_msgs::Odometry_<std::allocator<void> > >::SubscribeHandler<void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, void (mrs_uav_state_estimators::Correction<2>::*)(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>), mrs_uav_state_estimators::Correction<2>*)::{lambda()#1}::operator()() const0
mrs_lib::SubscribeHandler<std_msgs::Float64_<std::allocator<void> > >::SubscribeHandler<>(mrs_lib::SubscribeHandlerOptions const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)::{lambda()#1}::operator()() const164
+
+
+ + + +
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..12e016af0e --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.html @@ -0,0 +1,548 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/subscribe_handler.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - subscribe_handler.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:444989.8 %
Date:2024-04-09 22:03:33Functions:38774152.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 <ros/ros.h>
+      11             : #include <mrs_lib/timeout_manager.h>
+      12             : 
+      13             : namespace mrs_lib
+      14             : {
+      15             : 
+      16             :   static const ros::Duration no_timeout = ros::Duration(0);
+      17             : 
+      18             :   /* struct SubscribeHandlerOptions //{ */
+      19             :   
+      20             :   /**
+      21             :   * \brief A helper class to simplify setup of SubscribeHandler construction.
+      22             :   * This class is passed to the SubscribeHandler constructor and specifies its common options.
+      23             :   *
+      24             :   * \note Any option, passed directly to the SubscribeHandler constructor outside this structure, *OVERRIDES* values in this structure.
+      25             :   * The values in this structure can be thought of as default common values for all SubscribeHandler objects you want to create,
+      26             :   * and values passed directly to the constructor as specific options for the concrete handler.
+      27             :   *
+      28             :   */
+      29             :   struct SubscribeHandlerOptions
+      30             :   {
+      31             :     SubscribeHandlerOptions(const ros::NodeHandle& nh) : nh(nh) {}
+      32        2097 :     SubscribeHandlerOptions() = default;
+      33             :   
+      34             :     ros::NodeHandle nh;  /*!< \brief The ROS NodeHandle to be used for subscription. */
+      35             :   
+      36             :     std::string node_name = {};  /*!< \brief Name of the ROS node, using this handle (used for messages printed to console). */
+      37             :   
+      38             :     std::string topic_name = {};  /*!< \brief Name of the ROS topic to be handled. */
+      39             : 
+      40             :     std::shared_ptr<mrs_lib::TimeoutManager> timeout_manager = nullptr;  /*!< \brief Will be used for handling message timouts if necessary. If no manager is specified, it will be created with rate equal to half of \p no_message_timeout. */
+      41             :   
+      42             :     ros::Duration no_message_timeout = mrs_lib::no_timeout;  /*!< \brief If no new message is received for this duration, the \p timeout_callback function will be called. If \p timeout_callback is empty, an error message will be printed to the console. */
+      43             :   
+      44             :     std::function<void(const std::string& topic_name, const ros::Time& last_msg)> timeout_callback = {};  /*!< \brief This function will be called if no new message is received for the \p no_message_timeout duration. If this variable is empty, an error message will be printed to the console. */
+      45             :   
+      46             :     bool threadsafe = true;  /*!< \brief If true, all methods of the SubscribeHandler will be mutexed (using a recursive mutex) to avoid data races. */
+      47             :   
+      48             :     bool autostart = true;  /*!< \brief If true, the SubscribeHandler will be started after construction. Otherwise it has to be started using the start() method */
+      49             :   
+      50             :     uint32_t queue_size = 3;  /*!< \brief This parameter is passed to the NodeHandle when subscribing to the topic */
+      51             :   
+      52             :     ros::TransportHints transport_hints = ros::TransportHints();  /*!< \brief This parameter is passed to the NodeHandle when subscribing to the topic */
+      53             :   };
+      54             :   
+      55             :   //}
+      56             : 
+      57             :   /* SubscribeHandler class //{ */
+      58             :   /**
+      59             :   * \brief The main class for ROS topic subscription, message timeout handling etc.
+      60             :   *
+      61             :   * This class handles the raw ROS Subscriber for a specified topic. The last message received on the topic is remembered
+      62             :   * and may be retrieved by calling the getMsg() method. To check whether at least one message was received, use hasMsg()
+      63             :   * (if no message was received and you call getMsg(), a nullptr is returned and an error message is printed). To check
+      64             :   * whether a new message has arrived since the last call to getMsg(), use newMsg() (useful to check whether a new message
+      65             :   * needs to be processed in a loop or ROS Timer callback).
+      66             :   *
+      67             :   * A timeout callback function may be specified, which is called if no message is received on the topic for a specified
+      68             :   * timeout (use the \p timeout_callback and \p no_message_timeout parameters). If the timeout callback is not set by the
+      69             :   * user, an error message is printed to the console after the timeout by default.
+      70             :   *
+      71             :   * A message callback function may be specified, which is called whenever a new message is received (use the
+      72             :   * \p message_callback parameter).
+      73             :   *
+      74             :   * The callbacks and timeouts may be stopped and started using the stop() and start() methods.
+      75             :   *
+      76             :   * For more details, see the example below (I recommend starting with the simple_example which covers most use-cases).
+      77             :   *
+      78             :   */
+      79             :   template <typename MessageType>
+      80             :   class SubscribeHandler
+      81             :   {
+      82             :     public:
+      83             :     /*!
+      84             :       * \brief Convenience type for the template parameter to enable nice aliasing.
+      85             :       */
+      86             :       using message_type = MessageType;
+      87             : 
+      88             :     /*!
+      89             :       * \brief Type for the timeout callback function.
+      90             :       */
+      91             :       using timeout_callback_t = std::function<void(const std::string& topic_name, const ros::Time& last_msg)>;
+      92             : 
+      93             :     /*!
+      94             :       * \brief Convenience type for the message callback function.
+      95             :       */
+      96             :       using message_callback_t = std::function<void(typename MessageType::ConstPtr)>;
+      97             : 
+      98             :     public:
+      99             :     /*!
+     100             :       * \brief Returns the last received message on the topic, handled by this SubscribeHandler.
+     101             :       * Use hasMsg() first to check if at least one message is available or newMsg() to check if a new message
+     102             :       * since the last call to getMsg() is available.
+     103             :       *
+     104             :       * \return the last received message.
+     105             :       */
+     106     2441156 :       virtual typename MessageType::ConstPtr getMsg() {assert(m_pimpl); return m_pimpl->getMsg();};
+     107             : 
+     108             :     /*!
+     109             :       * \brief Returns the last received message on the topic without modifying the newMsg() or usedMsg() flags.
+     110             :       *
+     111             :       * \return the last received message.
+     112             :       */
+     113           0 :       virtual typename MessageType::ConstPtr peekMsg() const {assert(m_pimpl); return m_pimpl->peekMsg();};
+     114             : 
+     115             :     /*!
+     116             :       * \brief Used to check whether at least one message has been received on the handled topic.
+     117             :       *
+     118             :       * \return true if at least one message was received, otherwise false.
+     119             :       */
+     120     2396229 :       virtual bool hasMsg() const {assert(m_pimpl); return m_pimpl->hasMsg();};
+     121             : 
+     122             :     /*!
+     123             :       * \brief Used to check whether at least one message has been received on the handled topic since the last call to getMsg().
+     124             :       *
+     125             :       * \return true if at least one message was received, otherwise false.
+     126             :       */
+     127      469992 :       virtual bool newMsg() const {assert(m_pimpl); return m_pimpl->newMsg();};
+     128             : 
+     129             :     /*!
+     130             :       * \brief Used to check whether getMsg() was called at least once on this SubscribeHandler.
+     131             :       *
+     132             :       * \return true if getMsg() was called at least once, otherwise false.
+     133             :       */
+     134           0 :       virtual bool usedMsg() const {assert(m_pimpl); return m_pimpl->usedMsg();};
+     135             : 
+     136             :     /*!
+     137             :       * \brief Blocks until new data becomes available or until the timeout runs out or until a spurious wake-up.
+     138             :       *
+     139             :       * \return the message if a new message is available after waking up, \p nullptr otherwise.
+     140             :       */
+     141           0 :       virtual typename MessageType::ConstPtr waitForNew(const ros::WallDuration& timeout) {assert(m_pimpl); return m_pimpl->waitForNew(timeout);};
+     142             : 
+     143             :     /*!
+     144             :       * \brief Returns time of the last received message on the topic, handled by this SubscribeHandler.
+     145             :       *
+     146             :       * \return time when the last message was received.
+     147             :       */
+     148      579720 :       virtual ros::Time lastMsgTime() const {assert(m_pimpl); return m_pimpl->lastMsgTime();};
+     149             : 
+     150             :     /*!
+     151             :       * \brief Returns the resolved (full) name of the topic, handled by this SubscribeHandler.
+     152             :       *
+     153             :       * \return name of the handled topic.
+     154             :       */
+     155         288 :       virtual std::string topicName() const {assert(m_pimpl); return m_pimpl->topicName();};
+     156             : 
+     157             :     /*!
+     158             :       * \brief Returns the subscribed (unresolved) name of the topic, handled by this SubscribeHandler.
+     159             :       *
+     160             :       * \return name of the handled topic.
+     161             :       */
+     162           0 :       virtual std::string subscribedTopicName() const {assert(m_pimpl); return m_pimpl->m_topic_name;};
+     163             : 
+     164             :     /*!
+     165             :       * \brief Enables the callbacks for the handled topic.
+     166             :       *
+     167             :       * If the SubscribeHandler object is stopped using the stop() method, no callbacks will be called
+     168             :       * until the start() method is called.
+     169             :       */
+     170        5363 :       virtual void start() {assert(m_pimpl); return m_pimpl->start();};
+     171             : 
+     172             :     /*!
+     173             :       * \brief Disables the callbacks for the handled topic.
+     174             :       *
+     175             :       * All messages after this method is called will be ignored until start() is called again.
+     176             :       * Timeout checking will also be disabled.
+     177             :       */
+     178           5 :       virtual void stop() {assert(m_pimpl); return m_pimpl->stop();};
+     179             : 
+     180             :     public:
+     181             :     /*!
+     182             :       * \brief Default constructor to avoid having to use pointers.
+     183             :       *
+     184             :       * It does nothing and the SubscribeHandler it constructs will also do nothing.
+     185             :       * Use some of the other constructors for actual construction of a usable SubscribeHandler.
+     186             :       */
+     187        8159 :       SubscribeHandler() {};
+     188             : 
+     189             :     /*!
+     190             :       * \brief Main constructor.
+     191             :       *
+     192             :       * \param options    The common options struct (see documentation of SubscribeHandlerOptions).
+     193             :       * \param topic_name Name of the topic to be handled by this subscribed.
+     194             :       * \param args       Remaining arguments to be parsed (see other constructors).
+     195             :       *
+     196             :       */
+     197             :       template<class ... Types>
+     198        5359 :       SubscribeHandler(
+     199             :             const SubscribeHandlerOptions& options,
+     200             :             const std::string& topic_name,
+     201             :             Types ... args
+     202             :           )
+     203             :       :
+     204             :         SubscribeHandler(
+     205        5359 :             [options, topic_name]()
+     206             :             {
+     207       10718 :               SubscribeHandlerOptions opts = options;
+     208        5359 :               opts.topic_name = topic_name;
+     209       10718 :               return opts;
+     210             :             }(),
+     211             :             args...
+     212        5359 :             )
+     213             :       {
+     214        5359 :       }
+     215             : 
+     216             :     /*!
+     217             :       * \brief Convenience constructor overload.
+     218             :       *
+     219             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     220             :       * \param message_callback The callback function to call when a new message is received (you can leave this argument empty and just use the newMsg()/hasMsg() and getMsg() interface).
+     221             :       *
+     222             :       */
+     223        5359 :       SubscribeHandler(
+     224             :             const SubscribeHandlerOptions& options,
+     225             :             const message_callback_t& message_callback = {}
+     226             :           )
+     227        5359 :       {
+     228        5359 :         if (options.threadsafe)
+     229             :         {
+     230        5359 :           m_pimpl = std::make_unique<ImplThreadsafe>
+     231             :             (
+     232             :               options,
+     233             :               message_callback
+     234             :             );
+     235             :         }
+     236             :         else
+     237             :         {
+     238           0 :           m_pimpl = std::make_unique<Impl>
+     239             :             (
+     240             :               options,
+     241             :               message_callback
+     242             :             );
+     243             :         }
+     244        5359 :         if (options.autostart)
+     245        5358 :           start();
+     246        5359 :       };
+     247             : 
+     248             :     /*!
+     249             :       * \brief Convenience constructor overload.
+     250             :       *
+     251             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     252             :       * \param timeout_callback The callback function to call when a new message is not received for the duration, specified in \p options or in the \p no_message_timeout parameter.
+     253             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     254             :       *
+     255             :       */
+     256             :       template <class ... Types>
+     257           1 :       SubscribeHandler(
+     258             :             const SubscribeHandlerOptions& options,
+     259             :             const timeout_callback_t& timeout_callback,
+     260             :             Types ... args
+     261             :           )
+     262             :       :
+     263             :         SubscribeHandler(
+     264           1 :             [options, timeout_callback]()
+     265             :             {
+     266           2 :               SubscribeHandlerOptions opts = options;
+     267           1 :               opts.timeout_callback = timeout_callback;
+     268           2 :               return opts;
+     269             :             }(),
+     270             :             args...
+     271           1 :             )
+     272             :       {
+     273           1 :       }
+     274             : 
+     275             :     /*!
+     276             :       * \brief Convenience constructor overload.
+     277             :       *
+     278             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     279             :       * \param timeout_callback The callback method to call when a new message is not received for the duration, specified in \p options or in the \p no_message_timeout parameter.
+     280             :       * \param obj1             The object on which the callback method \p timeout_callback will be called.
+     281             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     282             :       *
+     283             :       */
+     284             :       template <class ObjectType1, class ... Types>
+     285             :       SubscribeHandler(
+     286             :             const SubscribeHandlerOptions& options,
+     287             :             void (ObjectType1::*const timeout_callback) (const std::string& topic_name, const ros::Time& last_msg),
+     288             :             ObjectType1* const obj1,
+     289             :             Types ... args
+     290             :           )
+     291             :       :
+     292             :         SubscribeHandler(
+     293             :             [options, timeout_callback, obj1]()
+     294             :             {
+     295             :               SubscribeHandlerOptions opts = options;
+     296             :               opts.timeout_callback = timeout_callback == nullptr ? timeout_callback_t() : std::bind(timeout_callback, obj1, std::placeholders::_1, std::placeholders::_2);
+     297             :               return opts;
+     298             :             }(),
+     299             :             args...
+     300             :             )
+     301             :       {
+     302             :       }
+     303             : 
+     304             :     /*!
+     305             :       * \brief Convenience constructor overload.
+     306             :       *
+     307             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     308             :       * \param message_callback The callback method to call when a new message is received.
+     309             :       * \param obj2             The object on which the callback method \p timeout_callback will be called.
+     310             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     311             :       *
+     312             :       */
+     313             :       template <class ObjectType2, class ... Types>
+     314        2273 :       SubscribeHandler(
+     315             :             const SubscribeHandlerOptions& options,
+     316             :             void (ObjectType2::*const message_callback) (typename MessageType::ConstPtr),
+     317             :             ObjectType2* const obj2,
+     318             :             Types ... args
+     319             :           )
+     320             :       :
+     321             :         SubscribeHandler(
+     322             :             options,
+     323        4546 :             message_callback == nullptr ? message_callback_t() : std::bind(message_callback, obj2, std::placeholders::_1),
+     324             :             args...
+     325        2273 :             )
+     326             :       {
+     327        2273 :       }
+     328             : 
+     329             :     /*!
+     330             :       * \brief Convenience constructor overload.
+     331             :       *
+     332             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     333             :       * \param message_callback The callback method to call when a new message is received.
+     334             :       * \param obj2             The object on which the callback method \p timeout_callback will be called.
+     335             :       * \param timeout_callback The callback method to call when a new message is not received for the duration, specified in \p options or in the \p no_message_timeout parameter.
+     336             :       * \param obj1             The object on which the callback method \p timeout_callback will be called.
+     337             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     338             :       *
+     339             :       */
+     340             :      template <class ObjectType1, class ObjectType2, class ... Types>
+     341             :      SubscribeHandler(
+     342             :            const SubscribeHandlerOptions& options,
+     343             :            void (ObjectType2::*const message_callback) (typename MessageType::ConstPtr),
+     344             :            ObjectType2* const obj2,
+     345             :            void (ObjectType1::*const timeout_callback) (const std::string& topic_name, const ros::Time& last_msg),
+     346             :            ObjectType1* const obj1,
+     347             :            Types ... args
+     348             :          )
+     349             :      :
+     350             :        SubscribeHandler(
+     351             :             [options, timeout_callback, obj1]()
+     352             :             {
+     353             :               SubscribeHandlerOptions opts = options;
+     354             :               opts.timeout_callback = timeout_callback == nullptr ? timeout_callback_t() : std::bind(timeout_callback, obj1, std::placeholders::_1, std::placeholders::_2);
+     355             :               return opts;
+     356             :             }(),
+     357             :             message_callback == nullptr ? message_callback_t() : std::bind(message_callback, obj2, std::placeholders::_1),
+     358             :             args...
+     359             :             )
+     360             :      {
+     361             :      }
+     362             : 
+     363             :     /*!
+     364             :       * \brief Convenience constructor overload.
+     365             :       *
+     366             :       * \param options            The common options struct (see documentation of SubscribeHandlerOptions).
+     367             :       * \param no_message_timeout If no message is received for this duration, the timeout callback function is called. If no timeout callback is specified, an error message is printed to the console.
+     368             :       * \param args               Remaining arguments to be parsed (see other constructors).
+     369             :       *
+     370             :       */
+     371             :       template<class ... Types>
+     372             :       SubscribeHandler(
+     373             :             const SubscribeHandlerOptions& options,
+     374             :             const ros::Duration& no_message_timeout,
+     375             :             Types ... args
+     376             :           )
+     377             :       :
+     378             :         SubscribeHandler(
+     379             :             [options, no_message_timeout]()
+     380             :             {
+     381             :               SubscribeHandlerOptions opts = options;
+     382             :               opts.no_message_timeout = no_message_timeout;
+     383             :               return opts;
+     384             :             }(),
+     385             :             args...
+     386             :             )
+     387             :       {
+     388             :       }
+     389             : 
+     390             :     /*!
+     391             :       * \brief Convenience constructor overload.
+     392             :       *
+     393             :       * \param options          The common options struct (see documentation of SubscribeHandlerOptions).
+     394             :       * \param timeout_manager  The manager for timeout callbacks.
+     395             :       * \param args             Remaining arguments to be parsed (see other constructors).
+     396             :       *
+     397             :       */
+     398             :       template <class ... Types>
+     399             :       SubscribeHandler(
+     400             :             const SubscribeHandlerOptions& options,
+     401             :             mrs_lib::TimeoutManager& timeout_manager,
+     402             :             Types ... args
+     403             :           )
+     404             :       :
+     405             :         SubscribeHandler(
+     406             :             options,
+     407             :             timeout_manager = timeout_manager,
+     408             :             args...
+     409             :             )
+     410             :       {
+     411             :       }
+     412             : 
+     413       13538 :       ~SubscribeHandler() = default;
+     414             :       // delete copy constructor and assignment operator (forbid copying shandlers)
+     415             :       SubscribeHandler(const SubscribeHandler&) = delete;
+     416             :       SubscribeHandler& operator=(const SubscribeHandler&) = delete;
+     417             :       // define only move constructor and assignemnt operator
+     418          20 :       SubscribeHandler(SubscribeHandler&& other)
+     419          20 :       {
+     420          20 :         this->m_pimpl = std::move(other.m_pimpl);
+     421          20 :         other.m_pimpl = nullptr;
+     422          20 :       }
+     423        5257 :       SubscribeHandler& operator=(SubscribeHandler&& other)
+     424             :       {
+     425        5257 :         this->m_pimpl = std::move(other.m_pimpl);
+     426        5257 :         other.m_pimpl = nullptr;
+     427        5257 :         return *this;
+     428             :       }
+     429             : 
+     430             :     private:
+     431             :       class Impl;
+     432             :       class ImplThreadsafe;
+     433             :       std::unique_ptr<Impl> m_pimpl;
+     434             :   };
+     435             :   //}
+     436             : 
+     437             : /*!
+     438             :   * \brief Helper alias for convenient extraction of handled message type from a SubscribeHandlerPtr.
+     439             :   */
+     440             :   template<typename SubscribeHandler>
+     441             :   using message_type = typename SubscribeHandler::message_type;
+     442             : 
+     443             : /*!
+     444             :   * \brief Helper function for object construstion e.g. in case of member objects.
+     445             :   * This function is useful to avoid specifying object template parameters twice - once in definition of the variable and second time during object construction.
+     446             :   * This function can deduce the template parameters from the type of the already defined object, because it returns the newly constructed object as a reference
+     447             :   * argument and not as a return type.
+     448             :   *
+     449             :   * \param object The object to be constructed.
+     450             :   * \param args   These arguments are passed to the object constructor.
+     451             :   */
+     452             :   template<typename Class, class ... Types>
+     453             :   void construct_object(
+     454             :         Class& object,
+     455             :         Types ... args
+     456             :       )
+     457             :   {
+     458             :     object = Class(args...);
+     459             :   }
+     460             : }
+     461             : 
+     462             : #include <mrs_lib/impl/subscribe_handler.hpp>
+     463             : 
+     464             : #endif // SUBRSCRIBE_HANDLER_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.overview.html b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.overview.html new file mode 100644 index 0000000000..056386c498 --- /dev/null +++ b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.overview.html @@ -0,0 +1,136 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/subscribe_handler.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.png b/mrs_lib/include/mrs_lib/subscribe_handler.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..b6e9ccb1d3175600727e604938e2df9a1db1b393 GIT binary patch literal 1783 zcmVGAf72gj3K5r zbz2ND5}}4FQEmJ&0qHg3YyO-C@r%I16(bOAcI{2~uIrUq&|?~*Fbke#KB6~6I8Vnr z*#`C}y~&UtGp8aqYx()_!9H3$vYLf#J|t)Of*?k(LiRez2b~a-7VDb>@sJ(OR zSPm&vvJ`~KR`Ok-ZVqcFgM8B6WXF=d6m!tz668ytM&>Z-6K5u6q!$AOc%jcH>3fS({;hPmwMn+C2MvDs~C)tCPOw1-3bupJYJgBin5(kdU98OtK zN-*pu2VTAz1vBYHiE0uiLlN>kL#(mB6FXs73unw;3sz_yc$`3{Fzh2wZJ5Y%O{q?O z6g)UZIm3|l&g)}FbTz%1oSPcWrg26f^ROQycTEwZjt5oF07LA8$5R1_ppf;LRN)NXH-nI=1IRZ;*ZXa1Jm$BBU5$hac6W< z>vHD0c*JRm88FCYuB0SXfeBrz8I(kg#WX;~w>RzSqhg>oXf`nCRNAM4{w5XvjlTQ9 zi<_&mKLc=%xTNe^@O*xL-tYJCpTGOpo!b|_@K;}}ps7tqC`HZ1s>Z;0N`fAZxipr! zT+JxJq3Mhs445)zWWji4^t|CQ!b8ANjVo;ChlhY6?T6suCbcma-FuA(#UuKr*Q4ycwyjT-@Af7jjOV# z1}X6BnolZtX|LeqUB29-p!+za)O=u=YT11ec{{UJ2gnCyJ$DS0)QGKqEA> zyv^&elvmKyrlVVS{8|Sfw;woTVV40RMpHY^Ip6CV;4}VE?-si#x8M(3=8X zL5s=>ou>R#tqPAJ6&N4mRxM#Slrw2_gpYNmF=oCUVf0`e;s!TL)>@Lh6M z1b;Q9l#Bu*NR%Sm|N54f;T}SbW~y~$N6c$RxX$#k;(w-iXq@Ae&+a*xjc`l|iCimSGlp%iNo zmj%`H0CA`Bg?F!~%P6IkY8v=0l;Y7Tx!)1|3+Xzm-Sze((0I;u;~&MXuQxstxT(KY zX(e(pqT9~%KW^TUqLm<}a6q|Gi{807Q{a<@olDO1>&?kSZ5w1K*wbh>t$iprRF+ck ziO$Ts4mAXAeRMQ)PEX@CnHIEgiUfFjEjoFq{_I)!@FN41v;5;w+=cL$!u>PPEJ%xJ zh?{xIOs}QF171_jPJ9*c4`*F09O$uQMu)yez?5JhmHe;jSDyG-brQgzcQ(Vv?+oq; zK?^@BX1I#M8`Opk0%sintuOW+}mrx)Rs|8>y8oYETAgY|8< Z{{wkhUB=a#p|$`3002ovPDHLkV1nx6SeyU= literal 0 HcmV?d00001 diff --git a/mrs_lib/include/mrs_lib/timeout_manager.h.func-sort-c.html b/mrs_lib/include/mrs_lib/timeout_manager.h.func-sort-c.html new file mode 100644 index 0000000000..82d13a8e1c --- /dev/null +++ b/mrs_lib/include/mrs_lib/timeout_manager.h.func-sort-c.html @@ -0,0 +1,80 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timeout_manager.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - timeout_manager.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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..e311f057ff --- /dev/null +++ b/mrs_lib/include/mrs_lib/timeout_manager.h.func.html @@ -0,0 +1,80 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timeout_manager.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - timeout_manager.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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..a81307fece --- /dev/null +++ b/mrs_lib/include/mrs_lib/timeout_manager.h.gcov.html @@ -0,0 +1,169 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timeout_manager.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - timeout_manager.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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          68 :       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      138729 :       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          72 :       void start(const timeout_id_t id, const ros::Time& time = ros::Time::now());
+      42             : 
+      43             :       void pauseAll();
+      44             : 
+      45           2 :       void startAll(const ros::Time& time = ros::Time::now());
+      46             : 
+      47             :       void change(const timeout_id_t id, const ros::Duration& timeout, const callback_t& callback, const ros::Time& last_reset = ros::Time::now(), const bool oneshot = false, const bool autostart = true);
+      48             : 
+      49             :       ros::Time lastReset(const timeout_id_t id);
+      50             : 
+      51             :       bool started(const timeout_id_t id);
+      52             : 
+      53             :       /* implementation details //{ */
+      54             :       
+      55             :       private:
+      56             :         // | ---------------------- private types --------------------- |
+      57             :         struct timeout_info_t
+      58             :         {
+      59             :           bool oneshot;
+      60             :           bool started;
+      61             :           callback_t callback;
+      62             :           ros::Duration timeout;
+      63             :           ros::Time last_reset;
+      64             :           ros::Time last_callback;
+      65             :         };
+      66             :       
+      67             :       private:
+      68             :         // | --------------------- private methods -------------------- |
+      69             :         void main_timer_callback([[maybe_unused]] const ros::TimerEvent& evt);
+      70             :       
+      71             :       private:
+      72             :         // | ------------------------- members ------------------------ |
+      73             :         std::recursive_mutex m_mtx;
+      74             :         timeout_id_t m_last_id;
+      75             :         std::vector<timeout_info_t> m_timeouts;
+      76             :       
+      77             :         ros::Timer m_main_timer;
+      78             :       
+      79             :       //}
+      80             : 
+      81             :   };
+      82             :   //}
+      83             : }
+      84             : 
+      85             : #endif // TIMEOUT_MANAGER_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/timeout_manager.h.gcov.overview.html b/mrs_lib/include/mrs_lib/timeout_manager.h.gcov.overview.html new file mode 100644 index 0000000000..bc8b5520c4 --- /dev/null +++ b/mrs_lib/include/mrs_lib/timeout_manager.h.gcov.overview.html @@ -0,0 +1,42 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/timeout_manager.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

+
          Line data    Source code
+
+       1             : #ifndef MRS_TIMER_H
+       2             : #define MRS_TIMER_H
+       3             : 
+       4             : #include <ros/ros.h>
+       5             : #include <thread>
+       6             : #include <mutex>
+       7             : #include <condition_variable>
+       8             : #include <atomic>
+       9             : 
+      10             : namespace mrs_lib
+      11             : {
+      12             :   bool breakable_sleep(const ros::Duration& dur, const std::atomic<bool>& continue_sleep);
+      13             : 
+      14             :   /**
+      15             :    * @brief Common wrapper representing the functionality of the ros::Timer.
+      16             :    *
+      17             :    * The implementation can then use either ros::Timer (the ROSTimer class)
+      18             :    * or threads and synchronization primitives from the C++ standard library
+      19             :    * (the ThreadTimer class). Both these variants implement the same interface.
+      20             :    *
+      21             :    * @note Functionality of the two implementations differs in some details.
+      22             :    */
+      23             :   class MRSTimer
+      24             :   {
+      25             :     public:
+      26             :     using callback_t = std::function<void(const ros::TimerEvent&)>;
+      27             : 
+      28             :     /**
+      29             :      * @brief stop the timer
+      30             :      */
+      31             :     virtual void stop() = 0;
+      32             : 
+      33             :     /**
+      34             :      * @brief start the timer
+      35             :      */
+      36             :     virtual void start() = 0;
+      37             : 
+      38             :     /**
+      39             :      * @brief set the timer period/duration
+      40             :      *
+      41             :      * @param duration
+      42             :      * @param reset
+      43             :      */
+      44             :     virtual void setPeriod(const ros::Duration& duration, const bool reset = true) = 0;
+      45             : 
+      46             :     /**
+      47             :      * @brief returns true if callbacks should be called
+      48             :      *
+      49             :      * @return true if timer is running
+      50             :      */
+      51             :     virtual bool running() = 0;
+      52             : 
+      53          16 :     virtual ~MRSTimer() = default;
+      54             :     MRSTimer(const MRSTimer&) = default;
+      55             :     MRSTimer(MRSTimer&&) = default;
+      56             :     MRSTimer& operator=(const MRSTimer&) = default;
+      57             :     MRSTimer& operator=(MRSTimer&&) = default;
+      58             : 
+      59             :     protected:
+      60          16 :     MRSTimer() = default;
+      61             :   };
+      62             : 
+      63             :   // | ------------------------ ROSTimer ------------------------ |
+      64             : 
+      65             :   /* class ROSTimer //{ */
+      66             : 
+      67             :   /**
+      68             :    * @brief ros::Timer wrapper. The interface is the same as with ros::Timer, except for the initialization method.
+      69             :    */
+      70             :   class ROSTimer : public MRSTimer {
+      71             :   public:
+      72             :     ROSTimer();
+      73             : 
+      74             :     /**
+      75             :      * @brief Constructs the object.
+      76             :      *
+      77             :      * @tparam ObjectType
+      78             :      * @param nh                            ROS node handle to be used for creating the underlying ros::Timer object.
+      79             :      * @param rate                          rate at which the callback should be called.
+      80             :      * @param ObjectType::*const callback   callback method to be called.
+      81             :      * @param obj                           object for the method.
+      82             :      * @param oneshot                       whether the callback should only be called once after starting.
+      83             :      * @param autostart                     whether the timer should immediately start after construction.
+      84             :      */
+      85             :     template <class ObjectType>
+      86             :     ROSTimer(const ros::NodeHandle& nh, const ros::Rate& rate, void (ObjectType::*const callback)(const ros::TimerEvent&), ObjectType* const obj,
+      87             :              const bool oneshot = false, const bool autostart = true);
+      88             : 
+      89             :     /**
+      90             :      * @brief full constructor
+      91             :      *
+      92             :      * @tparam ObjectType
+      93             :      * @param nh                            ROS node handle to be used for creating the underlying ros::Timer object.
+      94             :      * @param duration                      desired callback period.
+      95             :      * @param ObjectType::*const callback   callback method to be called.
+      96             :      * @param obj                           object for the method.
+      97             :      * @param oneshot                       whether the callback should only be called once after starting.
+      98             :      * @param autostart                     whether the timer should immediately start after construction.
+      99             :      */
+     100             :     template <class ObjectType>
+     101             :     ROSTimer(const ros::NodeHandle& nh, const ros::Duration& duration, void (ObjectType::*const callback)(const ros::TimerEvent&), ObjectType* const obj,
+     102             :              const bool oneshot = false, const bool autostart = true);
+     103             : 
+     104             :     /**
+     105             :      * @brief stop the timer
+     106             :      */
+     107             :     virtual void stop() override;
+     108             : 
+     109             :     /**
+     110             :      * @brief start the timer
+     111             :      */
+     112             :     virtual void start() override;
+     113             : 
+     114             :     /**
+     115             :      * @brief set the timer period/duration
+     116             :      *
+     117             :      * @param duration
+     118             :      * @param reset
+     119             :      */
+     120             :     virtual void setPeriod(const ros::Duration& duration, const bool reset = true) override;
+     121             : 
+     122             :     /**
+     123             :      * @brief returns true if callbacks should be called
+     124             :      *
+     125             :      * @return true if timer is running
+     126             :      */
+     127             :     virtual bool running() override;
+     128             : 
+     129          16 :     virtual ~ROSTimer() override {stop();};
+     130             :     // to keep rule of five since we have a custom destructor
+     131             :     ROSTimer(const ROSTimer&) = delete;
+     132             :     ROSTimer(ROSTimer&&) = delete;
+     133             :     ROSTimer& operator=(const ROSTimer&) = delete;
+     134             :     ROSTimer& operator=(ROSTimer&&) = delete;
+     135             : 
+     136             :   private:
+     137             :     std::mutex mutex_timer_;
+     138             : 
+     139             :     std::unique_ptr<ros::Timer> timer_;
+     140             :   };
+     141             : 
+     142             :   //}
+     143             : 
+     144             :   // | ----------------------- ThreadTimer ---------------------- |
+     145             : 
+     146             :   /* class ThreadTimer //{ */
+     147             : 
+     148             :   /**
+     149             :    * @brief Custom thread-based Timers with the same interface as mrs_lib::ROSTimer.
+     150             :    */
+     151             :   class ThreadTimer : public MRSTimer {
+     152             : 
+     153             :   public:
+     154             :     ThreadTimer();
+     155             : 
+     156             :     /**
+     157             :      * @brief Constructs the object.
+     158             :      *
+     159             :      * @tparam ObjectType
+     160             :      * @param nh                            ignored (used just for consistency with ROSTimer)
+     161             :      * @param rate                          rate at which the callback should be called.
+     162             :      * @param ObjectType::*const callback   callback method to be called.
+     163             :      * @param obj                           object for the method.
+     164             :      * @param oneshot                       whether the callback should only be called once after starting.
+     165             :      * @param autostart                     whether the timer should immediately start after construction.
+     166             :      */
+     167             :     template <class ObjectType>
+     168             :     ThreadTimer(const ros::NodeHandle& nh, const ros::Rate& rate, void (ObjectType::*const callback)(const ros::TimerEvent&), ObjectType* const obj,
+     169             :                 const bool oneshot = false, const bool autostart = true);
+     170             : 
+     171             :     /**
+     172             :      * @brief Constructs the object.
+     173             :      *
+     174             :      * @tparam ObjectType
+     175             :      * @param nh                            ignored (used just for consistency with ROSTimer)
+     176             :      * @param duration                      desired callback period.
+     177             :      * @param ObjectType::*const callback   callback method to be called.
+     178             :      * @param obj                           object for the method.
+     179             :      * @param oneshot                       whether the callback should only be called once after starting.
+     180             :      * @param autostart                     whether the timer should immediately start after construction.
+     181             :      */
+     182             :     template <class ObjectType>
+     183             :     ThreadTimer(const ros::NodeHandle& nh, const ros::Duration& duration, void (ObjectType::*const callback)(const ros::TimerEvent&), ObjectType* const obj,
+     184             :                 bool oneshot = false, const bool autostart = true);
+     185             : 
+     186             :     /**
+     187             :      * @brief stop the timer
+     188             :      *
+     189             :      * No more callbacks will be called after this method returns.
+     190             :      */
+     191             :     virtual void stop() override;
+     192             : 
+     193             :     /**
+     194             :      * @brief start the timer
+     195             :      *
+     196             :      * The first callback will be called in now + period.
+     197             :      *
+     198             :      * @note If the timer is already started, nothing will change.
+     199             :      */
+     200             :     virtual void start() override;
+     201             : 
+     202             :     /**
+     203             :      * @brief set the timer period/duration
+     204             :      *
+     205             :      * The new period will be applied after the next callback.
+     206             :      *
+     207             :      * @param duration the new desired callback period.
+     208             :      * @param reset    ignored in this implementation.
+     209             :      */
+     210             :     virtual void setPeriod(const ros::Duration& duration, const bool reset = true) override;
+     211             : 
+     212             :     /**
+     213             :      * @brief returns true if callbacks should be called
+     214             :      *
+     215             :      * @return true if timer is running
+     216             :      */
+     217             :     virtual bool running() override;
+     218             : 
+     219             :     /**
+     220             :      * @brief stops the timer and then destroys the object
+     221             :      *
+     222             :      * No more callbacks will be called when the destructor is started.
+     223             :      */
+     224          16 :     virtual ~ThreadTimer() override {stop();};
+     225             :     // to keep rule of five since we have a custom destructor
+     226             :     ThreadTimer(const ThreadTimer&) = delete;
+     227             :     ThreadTimer(ThreadTimer&&) = delete;
+     228             :     ThreadTimer& operator=(const ThreadTimer&) = delete;
+     229             :     ThreadTimer& operator=(ThreadTimer&&) = delete;
+     230             : 
+     231             :   private:
+     232             :     class Impl;
+     233             : 
+     234             :     std::unique_ptr<Impl> impl_;
+     235             : 
+     236             :   };  // namespace mrs_lib
+     237             : 
+     238             :   //}
+     239             : 
+     240             : #include <mrs_lib/impl/timer.hpp>
+     241             : 
+     242             : }  // namespace mrs_lib
+     243             : 
+     244             : #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..7f8814e032 --- /dev/null +++ b/mrs_lib/include/mrs_lib/timer.h.gcov.overview.html @@ -0,0 +1,81 @@ + + + + + + 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 + + +
+ 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..ce5e63f494d2e725baaa09e6f85316b2254598fa GIT binary patch literal 896 zcmV-`1AqL9P)(9B=S(B-K36%Z;|fgIa;!!G+K=VWZfW^vB2bc0kyqgH4RWh zpdf&O2l)ymcL1o(DIpZWSTv$Y)V!dwk@v&nBRdJ$xq$2^30m_Q>pSD+L4|P_C3Ol# z64EK?d*MTyJQlPkvg|0L2k;|}&R_@*!*~3_Jojb}1381qu?&IWIy5Gba~_X1`t#T_ zWZ5rb1aQH{O^`@bQOv>y9lP>K90Ntso-1e4GX-b2+vD+A9%(Ifxoxj3nAUwPn|?LlU9tJ6pZ+aw4zw}RSkB;<%`TnlpZn}cyMZtZJFFR5^&Hd(sZk`NAvA0ajZ=vB3z&@A@W|D>67pOz}X_&oXR+z=n-!Us3okOd}YgV{sg?_2@Ou?C+ z73w6gJuBRo^!Y-6`Le9v2rZl8C~D#P+!ha`-Wn>#f=AX(%bsJ6*H??2Vr(iNc)02Fstn$DDg=c3vZc_tfm2K zvxiYf(?R98W-hB~5D;+7o_-G8=#+fFEEYWT5l6|{O9}sxxe$a9V%^SFX#Po)X-P7^ zErWeWBpm!nS5YI=)Efx8q?=`yBAjm>!i&i6CMWD + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/transformer.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - transformer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:516085.0 %
Date:2024-04-09 22:03:33Functions: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&)258
mrs_lib::Transformer::retryLookupNewest(bool)411
mrs_lib::Transformer::resolveFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1032
mrs_lib::Transformer::setDefaultFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)412813
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)1724144
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&)1724218
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> > const&)1725956
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> > const&)1725966
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)1726462
+
+
+ + + +
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..a761dc74d4 --- /dev/null +++ b/mrs_lib/include/mrs_lib/transformer.h.func.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/transformer.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - transformer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:516085.0 %
Date:2024-04-09 22:03:33Functions: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&)1725956
mrs_lib::Transformer::frame_from[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)1724144
mrs_lib::Transformer::resolveFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1032
mrs_lib::Transformer::setDefaultFrame(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)412813
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&)1724218
mrs_lib::Transformer::setDefaultPrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)258
mrs_lib::Transformer::setLookupTimeout(ros::Duration)2
mrs_lib::Transformer::retryLookupNewest(bool)411
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&)1725966
mrs_lib::Transformer::frame_to[abi:cxx11](geometry_msgs::TransformStamped_<std::allocator<void> >&)1726462
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..607624b793 --- /dev/null +++ b/mrs_lib/include/mrs_lib/transformer.h.gcov.html @@ -0,0 +1,764 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/transformer.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/include/mrs_lib - transformer.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:516085.0 %
Date:2024-04-09 22:03:33Functions: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      412813 :     void setDefaultFrame(const std::string& frame_id)
+     131             :     {
+     132      825288 :       std::scoped_lock lck(mutex_);
+     133      412728 :       default_frame_id_ = frame_id;
+     134      412656 :     }
+     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         258 :     void setDefaultPrefix(const std::string& prefix)
+     154             :     {
+     155         516 :       std::scoped_lock lck(mutex_);
+     156         258 :       if (prefix.empty())
+     157           1 :         prefix_ = "";
+     158             :       else
+     159         257 :         prefix_ = prefix + "/";
+     160         258 :     }
+     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         411 :     void retryLookupNewest(const bool retry = true)
+     212             :     {
+     213         411 :       std::scoped_lock lck(mutex_);
+     214         411 :       retry_lookup_newest_ = retry;
+     215         411 :     }
+     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        1032 :     std::string resolveFrame(const std::string& frame_id)
+     249             :     {
+     250        2064 :       std::scoped_lock lck(mutex_);
+     251        2064 :       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     1725956 :     static constexpr const std::string& frame_from(const geometry_msgs::TransformStamped& msg)
+     551             :     {
+     552     1725956 :       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     1724144 :     static constexpr std::string& frame_from(geometry_msgs::TransformStamped& msg)
+     564             :     {
+     565     1724144 :       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     1725966 :     static constexpr const std::string& frame_to(const geometry_msgs::TransformStamped& msg)
+     575             :     {
+     576     1725966 :       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     1726462 :     static constexpr std::string& frame_to(geometry_msgs::TransformStamped& msg)
+     588             :     {
+     589     1726462 :       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     1724218 :     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     1724218 :       geometry_msgs::TransformStamped ret;
+     621     1724133 :       frame_from(ret) = from_frame;
+     622     1724205 :       frame_to(ret) = to_frame;
+     623     1724200 :       ret.header.stamp = time_stamp;
+     624     1724200 :       ret.transform = tf;
+     625     1724200 :       return ret;
+     626             :     }
+     627             :     //}
+     628             : 
+     629             :     /* copyChangeFrame() and related methods //{ */
+     630             : 
+     631             :     // helper type and member for detecting whether a message has a header using SFINAE
+     632             :     template<typename T>
+     633             :     using has_header_member_chk = decltype( std::declval<T&>().header );
+     634             :     template<typename T>
+     635             :     static constexpr bool has_header_member_v = std::experimental::is_detected<has_header_member_chk, T>::value;
+     636             :     
+     637             :     template <typename msg_t>
+     638             :     std_msgs::Header getHeader(const msg_t& msg);
+     639             :     template <typename pt_t>
+     640             :     std_msgs::Header getHeader(const pcl::PointCloud<pt_t>& cloud);
+     641             :     
+     642             :     template <typename msg_t>
+     643             :     void setHeader(msg_t& msg, const std_msgs::Header& header);
+     644             :     template <typename pt_t>
+     645             :     void setHeader(pcl::PointCloud<pt_t>& cloud, const std_msgs::Header& header);
+     646             :     
+     647             :     template <typename T>
+     648             :     T copyChangeFrame(const T& what, const std::string& frame_id);
+     649             :     
+     650             :     //}
+     651             : 
+     652             :     /* methods for converting between lattitude/longitude and UTM coordinates //{ */
+     653             :     geometry_msgs::Point LLtoUTM(const geometry_msgs::Point& what, const std::string& prefix);
+     654             :     geometry_msgs::PointStamped LLtoUTM(const geometry_msgs::PointStamped& what, const std::string& prefix);
+     655             :     geometry_msgs::Pose LLtoUTM(const geometry_msgs::Pose& what, const std::string& prefix);
+     656             :     geometry_msgs::PoseStamped LLtoUTM(const geometry_msgs::PoseStamped& what, const std::string& prefix);
+     657             :     
+     658             :     std::optional<geometry_msgs::Point> UTMtoLL(const geometry_msgs::Point& what, const std::string& prefix);
+     659             :     std::optional<geometry_msgs::PointStamped> UTMtoLL(const geometry_msgs::PointStamped& what, const std::string& prefix);
+     660             :     std::optional<geometry_msgs::Pose> UTMtoLL(const geometry_msgs::Pose& what, const std::string& prefix);
+     661             :     std::optional<geometry_msgs::PoseStamped> UTMtoLL(const geometry_msgs::PoseStamped& what, const std::string& prefix);
+     662             :     
+     663             :     // helper types and member for detecting whether the UTMtoLL and LLtoUTM methods are defined for a certain message
+     664             :     template<class Class, typename Message>
+     665             :     using UTMLL_method_chk = decltype(std::declval<Class>().UTMtoLL(std::declval<const Message&>(), ""));
+     666             :     template<class Class, typename Message>
+     667             :     using LLUTM_method_chk = decltype(std::declval<Class>().LLtoUTM(std::declval<const Message&>(), ""));
+     668             :     template<class Class, typename Message>
+     669             :     static constexpr bool UTMLL_exists_v = std::experimental::is_detected<UTMLL_method_chk, Class, Message>::value && std::experimental::is_detected<LLUTM_method_chk, Class, Message>::value;
+     670             :     //}
+     671             : 
+     672             :   };
+     673             : 
+     674             :   //}
+     675             : 
+     676             : }  // namespace mrs_lib
+     677             : 
+     678             : #include <mrs_lib/impl/transformer.hpp>
+     679             : 
+     680             : #endif  // TRANSFORMER_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/include/mrs_lib/transformer.h.gcov.overview.html b/mrs_lib/include/mrs_lib/transformer.h.gcov.overview.html new file mode 100644 index 0000000000..5aceca5a90 --- /dev/null +++ b/mrs_lib/include/mrs_lib/transformer.h.gcov.overview.html @@ -0,0 +1,190 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/include/mrs_lib/transformer.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

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

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

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

+ Overview +
+ + diff --git a/mrs_lib/include/mrs_lib/visual_object.h.gcov.png b/mrs_lib/include/mrs_lib/visual_object.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..2e10038ce016599658d08255c1ff1a58c48536b4 GIT binary patch literal 493 zcmV+saxyxDMeS`bZ>XVXLfMj6TyyoCqrtw2lm0u z+o(BT9)Ca$kB$N})=&j%ZYAN~M-2fl?xgk-O5E?%?$Wmj&*fS>a()0krI)3KDBPQ< zbwrxU^|#FxFTAkosWR(uv15M}_ueAI2C<4{orGw`5gG}@y*-=U|C$HgxkA&LlV~x;9avy^{ zD4UE5G>^=dcMGuSb&_NEgJF^XJ)xiRS!%!{##?Fl?i#WPsloKLN2!F_$XG*)sD*#d joDtA2469xit&98vAi`i14~K$;00000NkvXXu0mjfj#t*F literal 0 HcmV?d00001 diff --git a/mrs_lib/src/attitude_converter/attitude_converter.cpp.func-sort-c.html b/mrs_lib/src/attitude_converter/attitude_converter.cpp.func-sort-c.html new file mode 100644 index 0000000000..4d4c51d50d --- /dev/null +++ b/mrs_lib/src/attitude_converter/attitude_converter.cpp.func-sort-c.html @@ -0,0 +1,244 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter/attitude_converter.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converter - attitude_converter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-09 22:03:33Functions: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&)21969
mrs_lib::AttitudeConverter::getYawRateIntrinsic(double const&)90539
mrs_lib::AttitudeConverter::operator std::tuple<double&, double&, double&>()121595
mrs_lib::AttitudeConverter::getYaw()154524
mrs_lib::AttitudeConverter::calculateRPY()154529
mrs_lib::Vector3Converter::Vector3Converter(geometry_msgs::Vector3_<std::allocator<void> > const&)242145
mrs_lib::AttitudeConverter::setHeading(double const&)249511
mrs_lib::AttitudeConverter::getVectorZ()251508
mrs_lib::AttitudeConverter::getHeadingRate(mrs_lib::Vector3Converter const&)264110
mrs_lib::AttitudeConverter::operator tf2::Transform() const333639
mrs_lib::AttitudeConverter::operator tf2::Matrix3x3() const339408
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Quaternion<double, 0>)379773
mrs_lib::Vector3Converter::operator Eigen::Matrix<double, 3, 1, 0, 3, 1>() const515616
mrs_lib::AttitudeConverter::operator tf2::Quaternion() const775274
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Quaternion)843163
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Matrix<double, 3, 3, 0, 3, 3>)1042415
mrs_lib::AttitudeConverter::operator Eigen::Matrix<double, 3, 3, 0, 3, 3>() const1333027
mrs_lib::AttitudeConverter::getHeading()1419198
mrs_lib::AttitudeConverter::AttitudeConverter(geometry_msgs::Quaternion_<std::allocator<void> >)3139517
mrs_lib::AttitudeConverter::AttitudeConverter(double const&, double const&, double const&, mrs_lib::RPY_convention_t const&)6083569
mrs_lib::AttitudeConverter::operator geometry_msgs::Quaternion_<std::allocator<void> >() const7142208
mrs_lib::AttitudeConverter::validateOrientation()11486403
+
+
+ + + +
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..b062ff86c8 --- /dev/null +++ b/mrs_lib/src/attitude_converter/attitude_converter.cpp.func.html @@ -0,0 +1,244 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter/attitude_converter.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converter - attitude_converter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-09 22:03:33Functions: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&)242145
mrs_lib::Vector3Converter::Vector3Converter(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)21969
mrs_lib::Vector3Converter::Vector3Converter(double const&, double const&, double const&)1
mrs_lib::AttitudeConverter::getHeading()1419198
mrs_lib::AttitudeConverter::getVectorX()3
mrs_lib::AttitudeConverter::getVectorY()3
mrs_lib::AttitudeConverter::getVectorZ()251508
mrs_lib::AttitudeConverter::setHeading(double const&)249511
mrs_lib::AttitudeConverter::calculateRPY()154529
mrs_lib::AttitudeConverter::getHeadingRate(mrs_lib::Vector3Converter const&)264110
mrs_lib::AttitudeConverter::getExtrinsicRPY()1
mrs_lib::AttitudeConverter::getIntrinsicRPY()1
mrs_lib::AttitudeConverter::getYawRateIntrinsic(double const&)90539
mrs_lib::AttitudeConverter::validateOrientation()11486403
mrs_lib::AttitudeConverter::getYaw()154524
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> >)3139517
mrs_lib::AttitudeConverter::AttitudeConverter(tf::Quaternion)1
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Quaternion)843163
mrs_lib::AttitudeConverter::AttitudeConverter(tf2::Matrix3x3)1
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Quaternion<double, 0>)379773
mrs_lib::AttitudeConverter::AttitudeConverter(Eigen::Matrix<double, 3, 3, 0, 3, 3>)1042415
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&)6083569
mrs_lib::AttitudeConverter::operator std::tuple<double&, double&, double&>()121595
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>() const515616
mrs_lib::AttitudeConverter::operator geometry_msgs::Quaternion_<std::allocator<void> >() const7142208
mrs_lib::AttitudeConverter::operator tf::Quaternion() const1
mrs_lib::AttitudeConverter::operator tf2::Quaternion() const775274
mrs_lib::AttitudeConverter::operator tf2::Matrix3x3() const339408
mrs_lib::AttitudeConverter::operator tf2::Transform() const333639
mrs_lib::AttitudeConverter::operator Eigen::Matrix<double, 3, 3, 0, 3, 3>() const1333027
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..2dd4dccf72 --- /dev/null +++ b/mrs_lib/src/attitude_converter/attitude_converter.cpp.gcov.html @@ -0,0 +1,561 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter/attitude_converter.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converter - attitude_converter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-09 22:03:33Functions: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       21969 : Vector3Converter::Vector3Converter(const Eigen::Vector3d& vector3) {
+      33             : 
+      34       21969 :   vector3_[0] = vector3[0];
+      35       21969 :   vector3_[1] = vector3[1];
+      36       21969 :   vector3_[2] = vector3[2];
+      37       21969 : }
+      38             : 
+      39      242145 : Vector3Converter::Vector3Converter(const geometry_msgs::Vector3& vector3) {
+      40             : 
+      41      242110 :   vector3_[0] = vector3.x;
+      42      242126 :   vector3_[1] = vector3.y;
+      43      242085 :   vector3_[2] = vector3.z;
+      44      242095 : }
+      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      515616 : Vector3Converter::operator Eigen::Vector3d() const {
+      59             : 
+      60      515616 :   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     6083569 : AttitudeConverter::AttitudeConverter(const double& roll, const double& pitch, const double& yaw, const RPY_convention_t& format) {
+      79             : 
+      80     6080669 :   switch (format) {
+      81             : 
+      82     6080929 :     case RPY_EXTRINSIC: {
+      83     6080929 :       tf2_quaternion_.setRPY(roll, pitch, yaw);
+      84     6082171 :       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     6081911 :   validateOrientation();
+     104     6082278 : }
+     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     3139517 : AttitudeConverter::AttitudeConverter(const geometry_msgs::Quaternion quaternion) {
+     117     3139076 :   tf2_quaternion_.setX(quaternion.x);
+     118     3138535 :   tf2_quaternion_.setY(quaternion.y);
+     119     3139235 :   tf2_quaternion_.setZ(quaternion.z);
+     120     3139512 :   tf2_quaternion_.setW(quaternion.w);
+     121             : 
+     122     3139641 :   validateOrientation();
+     123     3138704 : }
+     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      379773 : AttitudeConverter::AttitudeConverter(const Eigen::Quaterniond quaternion) {
+     132      379773 :   tf2_quaternion_.setX(quaternion.x());
+     133      379773 :   tf2_quaternion_.setY(quaternion.y());
+     134      379773 :   tf2_quaternion_.setZ(quaternion.z());
+     135      379773 :   tf2_quaternion_.setW(quaternion.w());
+     136             : 
+     137      379773 :   validateOrientation();
+     138      379773 : }
+     139             : 
+     140     1042415 : AttitudeConverter::AttitudeConverter(const Eigen::Matrix3d matrix) {
+     141     1040731 :   Eigen::Quaterniond quaternion = Eigen::Quaterniond(matrix);
+     142             : 
+     143     1041069 :   tf2_quaternion_.setX(quaternion.x());
+     144     1040078 :   tf2_quaternion_.setY(quaternion.y());
+     145     1041045 :   tf2_quaternion_.setZ(quaternion.z());
+     146     1040792 :   tf2_quaternion_.setW(quaternion.w());
+     147             : 
+     148     1041033 :   validateOrientation();
+     149     1038290 : }
+     150             : 
+     151      843163 : AttitudeConverter::AttitudeConverter(const tf2::Quaternion quaternion) {
+     152             : 
+     153      841219 :   tf2_quaternion_ = quaternion;
+     154             : 
+     155      841219 :   validateOrientation();
+     156      841373 : }
+     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      775274 : AttitudeConverter::operator tf2::Quaternion() const {
+     170      775274 :   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     7142208 : AttitudeConverter::operator geometry_msgs::Quaternion() const {
+     186             : 
+     187     7142208 :   geometry_msgs::Quaternion geom_quaternion;
+     188             : 
+     189     7140154 :   geom_quaternion.x = tf2_quaternion_.x();
+     190     7134660 :   geom_quaternion.y = tf2_quaternion_.y();
+     191     7134868 :   geom_quaternion.z = tf2_quaternion_.z();
+     192     7136323 :   geom_quaternion.w = tf2_quaternion_.w();
+     193             : 
+     194     7135871 :   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     1333027 : AttitudeConverter::operator Eigen::Matrix3d() const {
+     206             : 
+     207     1333027 :   Eigen::Quaterniond quaternion(tf2_quaternion_.w(), tf2_quaternion_.x(), tf2_quaternion_.y(), tf2_quaternion_.z());
+     208             : 
+     209     2665517 :   return quaternion.toRotationMatrix();
+     210             : }
+     211             : 
+     212      121595 : AttitudeConverter::operator std::tuple<double&, double&, double&>() {
+     213             : 
+     214      121595 :   tf2::Matrix3x3(tf2_quaternion_).getRPY(roll_, pitch_, yaw_);
+     215             : 
+     216      121595 :   return std::tuple<double&, double&, double&>{roll_, pitch_, yaw_};
+     217             : }
+     218             : 
+     219      339408 : AttitudeConverter::operator tf2::Matrix3x3() const {
+     220             : 
+     221      339408 :   return tf2::Matrix3x3(tf2_quaternion_);
+     222             : }
+     223             : 
+     224      333639 : AttitudeConverter::operator tf2::Transform() const {
+     225             : 
+     226      333639 :   return tf2::Transform(tf2_quaternion_);
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* getters //{ */
+     232             : 
+     233      154524 : double AttitudeConverter::getYaw(void) {
+     234             : 
+     235      154524 :   calculateRPY();
+     236             : 
+     237      154524 :   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     1419198 : double AttitudeConverter::getHeading(void) {
+     255             : 
+     256     1419198 :   tf2::Vector3 b1 = tf2::Vector3(1, 0, 0);
+     257             : 
+     258     1418782 :   tf2::Vector3 x_new = tf2::Transform(tf2_quaternion_) * b1;
+     259             : 
+     260     1418412 :   if (fabs(x_new[0]) <= 1e-3 && fabs(x_new[1]) <= 1e-3) {
+     261           1 :     throw GetHeadingException();
+     262             :   }
+     263             : 
+     264     2834342 :   return atan2(x_new[1], x_new[0]);
+     265             : }
+     266             : 
+     267       90539 : 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       90539 :   if (fabs(heading_rate) < 1e-3) {
+     272       63470 :     return 0;
+     273             :   }
+     274             : 
+     275             :   // prep
+     276       27069 :   Eigen::Matrix3d R = *this;
+     277             : 
+     278             :   // construct the heading orbital velocity vector
+     279       27069 :   Eigen::Vector3d heading_vector   = Eigen::Vector3d(R(0, 0), R(1, 0), 0);
+     280       27069 :   Eigen::Vector3d orbital_velocity = Eigen::Vector3d(0, 0, heading_rate).cross(heading_vector);
+     281             : 
+     282             :   // projector to the heading orbital velocity vector subspace
+     283       27069 :   Eigen::Vector3d b_orb = Eigen::Vector3d(0, 0, 1).cross(heading_vector);
+     284       27069 :   b_orb.normalize();
+     285       27069 :   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       27069 :   Eigen::Vector3d projected = P * R.col(1);
+     289             : 
+     290             : 
+     291       27069 :   double orbital_velocity_norm = orbital_velocity.norm();
+     292       27069 :   double projected_norm        = projected.norm();
+     293             : 
+     294       27069 :   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       27069 :   double direction = mrs_lib::signum(orbital_velocity.dot(projected));
+     300             : 
+     301       27069 :   double output_yaw_rate = direction * (orbital_velocity_norm / projected_norm);
+     302             : 
+     303       27069 :   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       27069 :   return output_yaw_rate;
+     310             : }
+     311             : 
+     312      264110 : double AttitudeConverter::getHeadingRate(const Vector3Converter& attitude_rate) {
+     313             : 
+     314             :   // prep
+     315      264110 :   Eigen::Matrix3d R = *this;
+     316      264103 :   Eigen::Vector3d w = attitude_rate;
+     317             : 
+     318             :   // create the angular velocity tensor
+     319      264024 :   Eigen::Matrix3d W;
+     320      264008 :   W << 0, -w[2], w[1], w[2], 0, -w[0], -w[1], w[0], 0;
+     321             : 
+     322             :   // R derivative
+     323      264000 :   Eigen::Matrix3d R_d = R * W;
+     324             : 
+     325             :   // atan2 derivative
+     326      264070 :   double rx = R(0, 0);  // x-component of body X
+     327      263996 :   double ry = R(1, 0);  // y-component of body Y
+     328             : 
+     329      264013 :   double denom = rx * rx + ry * ry;
+     330             : 
+     331      264013 :   if (fabs(denom) <= 1e-5) {
+     332           0 :     ROS_ERROR("[AttitudeConverter]: getHeadingRate(): denominator near zero!!!");
+     333           0 :     throw MathErrorException();
+     334             :   }
+     335             : 
+     336      264013 :   double atan2_d_x = -ry / denom;
+     337      264013 :   double atan2_d_y = rx / denom;
+     338             : 
+     339             :   // atan2 total differential
+     340      264013 :   double heading_rate = atan2_d_x * R_d(0, 0) + atan2_d_y * R_d(1, 0);
+     341             : 
+     342      264041 :   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      251508 : Vector3Converter AttitudeConverter::getVectorZ(void) {
+     364             : 
+     365      251508 :   tf2::Vector3 b3 = tf2::Vector3(0, 0, 1);
+     366             : 
+     367      251510 :   tf2::Vector3 new_b3 = tf2::Transform(tf2_quaternion_) * b3;
+     368             : 
+     369      503020 :   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      249511 : AttitudeConverter AttitudeConverter::setHeading(const double& heading) {
+     404             : 
+     405             :   // get the Z unit vector after the original rotation
+     406      249511 :   Eigen::Vector3d b3 = getVectorZ();
+     407             : 
+     408             :   // check for singularity: z component of the thrust vector is 0
+     409      249504 :   if (fabs(b3[2]) < 1e-3) {
+     410           6 :     throw SetHeadingException();
+     411             :   }
+     412             : 
+     413             :   // get the desired heading as a vector in 3D
+     414      249500 :   Eigen::Vector3d h(cos(heading), sin(heading), 0);
+     415             : 
+     416      249502 :   Eigen::Matrix3d new_R;
+     417             : 
+     418      249501 :   new_R.col(2) = b3;
+     419             : 
+     420             :   // construct the oblique projection
+     421      249499 :   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      497906 :   Eigen::MatrixXd A = Eigen::MatrixXd(3, 2);
+     425      249306 :   A.col(0)          = projector_body_z_compl.col(0);
+     426      249229 :   A.col(1)          = projector_body_z_compl.col(1);
+     427             : 
+     428             :   // create the basis of the projection null-space complement
+     429      497445 :   Eigen::MatrixXd B = Eigen::MatrixXd(3, 2);
+     430      249333 :   B.col(0)          = Eigen::Vector3d(1, 0, 0);
+     431      249150 :   B.col(1)          = Eigen::Vector3d(0, 1, 0);
+     432             : 
+     433             :   // oblique projector to <range_basis>
+     434      496746 :   Eigen::MatrixXd Bt_A               = B.transpose() * A;
+     435      497351 :   Eigen::MatrixXd Bt_A_pseudoinverse = ((Bt_A.transpose() * Bt_A).inverse()) * Bt_A.transpose();
+     436      248535 :   Eigen::MatrixXd oblique_projector  = A * Bt_A_pseudoinverse * B.transpose();
+     437             : 
+     438      248693 :   new_R.col(0) = oblique_projector * h;
+     439      247991 :   new_R.col(0).normalize();
+     440             : 
+     441             :   // | ------------------------- body y ------------------------- |
+     442             : 
+     443      248427 :   new_R.col(1) = new_R.col(2).cross(new_R.col(0));
+     444      246643 :   new_R.col(1).normalize();
+     445             : 
+     446      496529 :   return AttitudeConverter(new_R);
+     447             : }
+     448             : 
+     449             : //}
+     450             : 
+     451             : // | ------------------------ internal ------------------------ |
+     452             : 
+     453             : /* calculateRPY() //{ */
+     454             : 
+     455      154529 : void AttitudeConverter::calculateRPY(void) {
+     456             : 
+     457      154529 :   if (!got_rpy_) {
+     458             : 
+     459      154529 :     tf2::Matrix3x3(tf2_quaternion_).getRPY(roll_, pitch_, yaw_);
+     460             :   }
+     461      154529 : }
+     462             : 
+     463             : //}
+     464             : 
+     465             : /* validateOrientation() //{ */
+     466             : 
+     467    11486403 : void AttitudeConverter::validateOrientation(void) {
+     468             : 
+     469    22965627 :   if (!std::isfinite(tf2_quaternion_.x()) || !std::isfinite(tf2_quaternion_.y()) || !std::isfinite(tf2_quaternion_.z()) ||
+     470    11473591 :       !std::isfinite(tf2_quaternion_.w())) {
+     471           2 :     throw InvalidAttitudeException();
+     472             :   }
+     473    11478134 : }
+     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..38a4ec9936 --- /dev/null +++ b/mrs_lib/src/attitude_converter/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-09 22:03:33Functions: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..0237ea1eaf --- /dev/null +++ b/mrs_lib/src/attitude_converter/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-09 22:03:33Functions: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..c91466fa6b --- /dev/null +++ b/mrs_lib/src/attitude_converter/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-09 22:03:33Functions: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..25ffad851b --- /dev/null +++ b/mrs_lib/src/attitude_converter/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-09 22:03:33Functions: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..0a705b04ca --- /dev/null +++ b/mrs_lib/src/attitude_converter/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-09 22:03:33Functions: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..4f17b11842 --- /dev/null +++ b/mrs_lib/src/attitude_converter/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/attitude_converter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/attitude_converterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21222494.6 %
Date:2024-04-09 22:03:33Functions: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..d9addc5a18 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.func-sort-c.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/batch_visualizer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizer - batch_visualizer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:16221774.7 %
Date:2024-04-09 22:03:33Functions:132356.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.png b/mrs_lib/src/batch_visualizer/batch_visualizer.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..589d1dd69d27c156fbd83eb79cd9d9dd81c37ada GIT binary patch literal 1265 zcmV+#se&lDtK+-3^Gtsn^m|xG#8Agq4!f7`=a8$U5a@V)VUr;zei_G4=J>X%pO!)krPgM-N+0BR@@@4mq2rL%fA4?xSImT1L+D|xY)=HV;wS0c)dkn zTg0TyykwEQrv&1`Vq86>e%?_Jct#rl5dE~#43R}E3L4ngPuoUBKd@<_gNgu9w=pkM z>M^oFRdGEKRphucG3%E0gLI#sktz=~z0uEr_zBWQc@HOVZ8K6Cx6*T9w!=KiGzBn` zuZ5GgTHp|mHqt)`CL12TuQi!1j}d4$N)5M>B;1pJC&K9v01#g7vBK3Y;d=zy&3*>q z%2Vzuwanj%a9ZF1;i~y!sBH*G1pXf3AmcY8oNo)bEw{YM)x;Zj zrx3uR*=6u2{6GfitF2XAYAe?`zKDRn^zXL~4>AHn7`^1Hg$kcH21soHUKr zIRNCEHmX>|!{qUk-DjVbiJ}nhDTU%GGr8qTaXL9nr=6&D4Dblg#o{`4aX5e~-RtWG z`=eHO+1>j53Omm`&T};g`?kM$c)wRh1nw45L z4kY{9ym69%+8NXxs*SOopAJ74S*tCG$e&s%t)Ae9#94zc6F{EAdFSaf-*z0t3V&0%R6IxX3D3 z9eB*7>Leb?+-y#dIFoSJ3<8;@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19243544.1 %
Date:2024-04-09 22:03:33Functions: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..c009520fd3 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19243544.1 %
Date:2024-04-09 22:03:33Functions: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..41dcfd691c --- /dev/null +++ b/mrs_lib/src/batch_visualizer/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19243544.1 %
Date:2024-04-09 22:03:33Functions: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..6e981c2d38 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19243544.1 %
Date:2024-04-09 22:03:33Functions: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..2471054d1c --- /dev/null +++ b/mrs_lib/src/batch_visualizer/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19243544.1 %
Date:2024-04-09 22:03:33Functions: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..a40736abef --- /dev/null +++ b/mrs_lib/src/batch_visualizer/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19243544.1 %
Date:2024-04-09 22:03:33Functions: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..83ac556c06 --- /dev/null +++ b/mrs_lib/src/batch_visualizer/visual_object.cpp.func-sort-c.html @@ -0,0 +1,168 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/batch_visualizer/visual_object.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/batch_visualizer - visual_object.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3021813.8 %
Date:2024-04-09 22:03:33Functions:72231.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

+ Overview +
+ + diff --git a/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.png b/mrs_lib/src/batch_visualizer/visual_object.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..b55f768965809edc613746889e563a4b1d7241f7 GIT binary patch literal 1229 zcmV;;1Ty=HP)2Nbs` zoTWIMm+F|wabY=fTua7yD)2OcZ?1tqZfc870j#l2Yw;o2ic`x%3rtvl0Lx4PG`AX> zSp(Y;p0(%cBnKv%DmNZ|4n&W5{FpRoIqT;Gxdu>ZXVGxGz%vc^5Lf8?!DL)P<*I@5 zT-jfvkf~5LBdgkHnks(V$ZzVIdDk+qANw=*M+J*P)A!J7s}Gn8z(w@s}=n{PM>>>MLb z;));`^*Izs%PdZxQL$rMD`g|Pj^LI9jWNKc=)j_ZG9A`thB>x`VVi`M2b^oV>-U>T zGyCMIic~Xi5$HML#Ir|^ZOT!OJ<8wY*MdCdS+#urW46DGJx)b|>OOhea=t2RHPAe} zbZcExHZN(OfCtu%?41D4X7mWGfr-zV8LDQ+!xo0F1!};|GwguD9;>>GNQR|GA)Zxe zj=lztn_2HAQhS>!&cOvW7FRqj}<;${}1n(02btrfqpw-A4y&R z)_Oi6NcBP$XFaKqYFJM|C~s-5<{Ge`M8~~gH>%3LjDiR5HHvmr;f=yw8mlLc|Q1EgcU0*vOlLx#mV%xQ`hWm90;EW<-=PckKX&-L<=B1XAnLSH;fvDBj9-*#5_Uj)e4#yy3u#JrPrkvOg~3Zg_6&wTMgM zj|Vo=%;SLnI^u44y0jH>$<0a#bVb~(M-kJfM_kcYPQ0lU_D{*(Z&&WKe)>Lp3ESD% zINWBB#s&iS4T$#chy)bxRXFPHD%k3lHo@?G2d?wYCjO-8U_PH~fwG^4vf~~gB! + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/conversions.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - conversions.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:324669.6 %
Date:2024-04-09 22:03:33Functions: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&)102114
mrs_lib::geometry::fromEigen(Eigen::Quaternion<double, 0> const&)102114
+
+
+ + + +
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..1e16ce46a2 --- /dev/null +++ b/mrs_lib/src/geometry/conversions.cpp.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/conversions.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - conversions.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:324669.6 %
Date:2024-04-09 22:03:33Functions: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&)102114
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&)102114
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..df520a3eaa --- /dev/null +++ b/mrs_lib/src/geometry/conversions.cpp.gcov.html @@ -0,0 +1,178 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/conversions.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - conversions.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:324669.6 %
Date:2024-04-09 22:03:33Functions: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      102114 :     geometry_msgs::Quaternion fromEigen(const Eigen::Quaterniond& what)
+      48             :     {
+      49      102114 :       geometry_msgs::Quaternion q;
+      50      102114 :       q.x = what.x();
+      51      102114 :       q.y = what.y();
+      52      102114 :       q.z = what.z();
+      53      102114 :       q.w = what.w();
+      54      102114 :       return q;
+      55             :     }
+      56             :     
+      57      102114 :     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      102114 :       Eigen::Quaterniond q;
+      61      102114 :       q.x() = what.x;
+      62      102114 :       q.y() = what.y;
+      63      102114 :       q.z() = what.z;
+      64      102114 :       q.w() = what.w;
+      65      102114 :       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..9b99b3b206 --- /dev/null +++ b/mrs_lib/src/geometry/index-detail-sort-f.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6744515.1 %
Date:2024-04-09 22:03:33Functions: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..719941e443 --- /dev/null +++ b/mrs_lib/src/geometry/index-detail-sort-l.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6744515.1 %
Date:2024-04-09 22:03:33Functions: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..79d97f0de6 --- /dev/null +++ b/mrs_lib/src/geometry/index-detail.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6744515.1 %
Date:2024-04-09 22:03:33Functions: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..8e91ba4e44 --- /dev/null +++ b/mrs_lib/src/geometry/index-sort-f.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6744515.1 %
Date:2024-04-09 22:03:33Functions: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..4f92ecf6f0 --- /dev/null +++ b/mrs_lib/src/geometry/index-sort-l.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6744515.1 %
Date:2024-04-09 22:03:33Functions: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..82154d3afc --- /dev/null +++ b/mrs_lib/src/geometry/index.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometryHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6744515.1 %
Date:2024-04-09 22:03:33Functions: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..57bd28628d --- /dev/null +++ b/mrs_lib/src/geometry/misc.cpp.func-sort-c.html @@ -0,0 +1,144 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/misc.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - misc.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:356454.7 %
Date:2024-04-09 22:03:33Functions:81650.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::geometry::solidAngle(double, double, double)0
mrs_lib::geometry::invHaversin(double)0
mrs_lib::geometry::triangleArea(double, double, double)0
mrs_lib::geometry::quaternionBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double)0
mrs_lib::geometry::quaternionFromEuler(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_lib::geometry::quaternionFromEuler(double, double, double)0
mrs_lib::geometry::sphericalTriangleArea(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)0
mrs_lib::geometry::haversin(double)0
mrs_lib::geometry::angleBetween(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)3
mrs_lib::geometry::rotationBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double)3
mrs_lib::geometry::angleaxisBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double)3
mrs_lib::geometry::cross(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1>)3
mrs_lib::geometry::quaternionFromHeading(double)102120
mrs_lib::geometry::angleBetween(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)130614
mrs_lib::geometry::dist(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)179466
mrs_lib::geometry::dist(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&)297280
+
+
+ + + +
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..88cc948099 --- /dev/null +++ b/mrs_lib/src/geometry/misc.cpp.func.html @@ -0,0 +1,144 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/misc.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - misc.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:356454.7 %
Date:2024-04-09 22:03:33Functions: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&)130614
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)102120
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&)297280
mrs_lib::geometry::dist(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)179466
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..6e0c8df633 --- /dev/null +++ b/mrs_lib/src/geometry/misc.cpp.gcov.html @@ -0,0 +1,269 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/misc.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/geometry - misc.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:356454.7 %
Date:2024-04-09 22:03:33Functions: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      130614 :     double angleBetween(const vec3_t& vec1, const vec3_t& vec2)
+      29             :     {
+      30      130614 :       const double sin_12 = vec1.cross(vec2).norm();
+      31      130623 :       const double cos_12 = vec1.dot(vec2);
+      32      130580 :       const double angle = std::atan2(sin_12, cos_12);
+      33      130580 :       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      102120 :     quat_t quaternionFromHeading(const double heading)
+      93             :     {
+      94      204240 :       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      297280 :     double dist(const vec2_t& a, const vec2_t& b)
+     171             :     {
+     172             : 
+     173      297280 :       return (a - b).norm();
+     174             :     }
+     175             : 
+     176      179466 :     double dist(const vec3_t& a, const vec3_t& b)
+     177             :     {
+     178             : 
+     179      179466 :       return (a - b).norm();
+     180             :     }
+     181             : 
+     182             :     //}
+     183             : 
+     184             :   }  // namespace geometry
+     185             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/geometry/misc.cpp.gcov.overview.html b/mrs_lib/src/geometry/misc.cpp.gcov.overview.html new file mode 100644 index 0000000000..bdcd7dc00e --- /dev/null +++ b/mrs_lib/src/geometry/misc.cpp.gcov.overview.html @@ -0,0 +1,67 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/geometry/misc.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

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

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

+ Overview +
+ + diff --git a/mrs_lib/src/math/math.cpp.gcov.png b/mrs_lib/src/math/math.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..11021a1f7a3fe1b16c9ed60780115ac61956a437 GIT binary patch literal 388 zcmeAS@N?(olHy`uVBq!ia0vp^0YL1?!VDz0Y_(eeq$C1-LR|m<|Gx?dmcNgUef6J# zVHHpuOr7)IycEdhEbxddW?X?_wfUqO7#M{-T^vI^I^TxHN;NC+EH*!&eSr7D zzjEnZM%SO5_wU^Pv> + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-09 22:03:33Functions: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..8186a2b74e --- /dev/null +++ b/mrs_lib/src/median_filter/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-09 22:03:33Functions: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..cbe93244ed --- /dev/null +++ b/mrs_lib/src/median_filter/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-09 22:03:33Functions: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..d762ef181e --- /dev/null +++ b/mrs_lib/src/median_filter/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-09 22:03:33Functions: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..4b8b3db25b --- /dev/null +++ b/mrs_lib/src/median_filter/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-09 22:03:33Functions: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..f40da99602 --- /dev/null +++ b/mrs_lib/src/median_filter/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-09 22:03:33Functions: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..db9d62499a --- /dev/null +++ b/mrs_lib/src/median_filter/median_filter.cpp.func-sort-c.html @@ -0,0 +1,148 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter/median_filter.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filter - median_filter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-09 22:03:33Functions: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&&)68
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter const&)69
mrs_lib::MedianFilter::MedianFilter(unsigned long, double, double, double)71
mrs_lib::MedianFilter::check(double)123934
mrs_lib::MedianFilter::median() const123951
mrs_lib::MedianFilter::full() const130548
mrs_lib::MedianFilter::add(double)130672
+
+
+ + + +
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..8cd78954b7 --- /dev/null +++ b/mrs_lib/src/median_filter/median_filter.cpp.func.html @@ -0,0 +1,148 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter/median_filter.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filter - median_filter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-09 22:03:33Functions: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)130672
mrs_lib::MedianFilter::check(double)123934
mrs_lib::MedianFilter::clear()2
mrs_lib::MedianFilter::addCheck(double)26
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter&&)68
mrs_lib::MedianFilter::MedianFilter(mrs_lib::MedianFilter const&)1
mrs_lib::MedianFilter::MedianFilter(unsigned long, double, double, double)71
mrs_lib::MedianFilter::MedianFilter()1
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter&&)0
mrs_lib::MedianFilter::operator=(mrs_lib::MedianFilter const&)69
mrs_lib::MedianFilter::initialized() const1
mrs_lib::MedianFilter::full() const130548
mrs_lib::MedianFilter::median() const123951
+
+
+ + + +
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..5416795de1 --- /dev/null +++ b/mrs_lib/src/median_filter/median_filter.cpp.gcov.html @@ -0,0 +1,286 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter/median_filter.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/median_filter - median_filter.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:829289.1 %
Date:2024-04-09 22:03:33Functions: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          71 :   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          71 :       m_max_diff(max_diff)
+      12             :   {
+      13          71 :     m_buffer.set_capacity(buffer_length);
+      14          71 :     m_buffer_sorted.reserve(buffer_length);
+      15          71 :   }
+      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          68 :   MedianFilter::MedianFilter(MedianFilter&& other)
+      32             :   {
+      33          68 :     *this = other;
+      34          68 :   }
+      35             : 
+      36             :   //}
+      37             : 
+      38             :   /* operator=() method and overloads //{ */
+      39          69 :   MedianFilter& MedianFilter::operator=(const MedianFilter& other)
+      40             :   {
+      41          69 :     std::scoped_lock lck(other.m_mtx, m_mtx);
+      42             :   
+      43          69 :     m_buffer = other.m_buffer;
+      44          69 :     m_buffer_sorted = other.m_buffer_sorted;
+      45          69 :     m_median = other.m_median;
+      46             :   
+      47             :     // parameters specified by the user
+      48          69 :     m_min_valid = other.m_min_valid;
+      49          69 :     m_max_valid = other.m_max_valid;
+      50          69 :     m_max_diff = other.m_max_diff;
+      51             :   
+      52         138 :     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      130672 :   void MedianFilter::add(const double value)
+      74             :   {
+      75      261205 :     std::scoped_lock lck(m_mtx);
+      76             :     // add the value to the buffer
+      77      130647 :     m_buffer.push_back(value);
+      78             :     // reset the cached median value
+      79      130568 :     m_median = std::nullopt;
+      80      130612 :   }
+      81             :   //}
+      82             : 
+      83             :   /* check() method //{ */
+      84      123934 :   bool MedianFilter::check(const double value)
+      85             :   {
+      86      123934 :     std::scoped_lock lck(m_mtx);
+      87             :     // check if all constraints are met
+      88      123975 :     const double diff = m_buffer.empty() ? 0.0 : std::abs(median() - value);
+      89      247929 :     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      130548 :   bool MedianFilter::full() const
+     113             :   {
+     114      261159 :     std::scoped_lock lck(m_mtx);
+     115      261209 :     return m_buffer.full();
+     116             :   }
+     117             :   //}
+     118             : 
+     119             :   /* median() method //{ */
+     120      123951 :   double MedianFilter::median() const
+     121             :   {
+     122      247945 :     std::scoped_lock lck(m_mtx);
+     123             :     // if the value was already calculated, just return it
+     124      123976 :     if (m_median.has_value())
+     125          19 :       return m_median.value();
+     126             :   
+     127             :     // check if there are even any numbers to calculate the median from
+     128      123993 :     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      123992 :     m_buffer_sorted.clear();
+     136             :     // copy all elements from the input buffer to buffer_sorted
+     137      124012 :     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      123972 :     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      123984 :     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      123913 :     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      124004 :     if (even_set)
+     148      123988 :       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      123960 :     return m_median.value();
+     154             :   }
+     155             :   //}
+     156             : 
+     157             :   /* initialized() method //{ */
+     158           1 :   bool MedianFilter::initialized() const
+     159             :   {
+     160           1 :     std::scoped_lock lck(m_mtx);
+     161           2 :     return m_buffer.size() > 0;
+     162             :   }
+     163             :   //}
+     164             : 
+     165             :   /* setBufferLength() method //{ */
+     166           2 :   void MedianFilter::setBufferLength(const size_t buffer_length)
+     167             :   {
+     168           4 :     std::scoped_lock lck(m_mtx);
+     169             :     // the median may change if the some values are discarded
+     170           2 :     if (buffer_length < m_buffer.size())
+     171           0 :       m_median = std::nullopt;
+     172             :   
+     173           2 :     m_buffer.set_capacity(buffer_length);
+     174           2 :     m_buffer_sorted.reserve(buffer_length);
+     175           2 :   }
+     176             :   //}
+     177             : 
+     178             :   /* setMinValue() method //{ */
+     179           2 :   void MedianFilter::setMinValue(const double min_value)
+     180             :   {
+     181           2 :     std::scoped_lock lck(m_mtx);
+     182           2 :     m_min_valid = min_value;
+     183           2 :   }
+     184             :   //}
+     185             : 
+     186             :   /* setMaxValue() method //{ */
+     187           2 :   void MedianFilter::setMaxValue(const double max_value)
+     188             :   {
+     189           2 :     std::scoped_lock lck(m_mtx);
+     190           2 :     m_max_valid = max_value;
+     191           2 :   }
+     192             :   //}
+     193             : 
+     194             :   /* setMaxDifference() method //{ */
+     195           2 :   void MedianFilter::setMaxDifference(const double max_diff)
+     196             :   {
+     197           2 :     std::scoped_lock lck(m_mtx);
+     198           2 :     m_max_diff = max_diff;
+     199           2 :   }
+     200             :   //}
+     201             : 
+     202             : } // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/median_filter/median_filter.cpp.gcov.overview.html b/mrs_lib/src/median_filter/median_filter.cpp.gcov.overview.html new file mode 100644 index 0000000000..f9295bd144 --- /dev/null +++ b/mrs_lib/src/median_filter/median_filter.cpp.gcov.overview.html @@ -0,0 +1,71 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/median_filter/median_filter.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/median_filter/median_filter.cpp.gcov.png b/mrs_lib/src/median_filter/median_filter.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..b9ea102b4e20dc14f5089f33c629964d0722e495 GIT binary patch literal 872 zcmV-u1DE`XP)?#D47~zV4R{20qwmlQQ?*Cb&s)6T zPH}7sC3BcfAv=e&rjVxZbZhD~ zU^@f$!VY5T54!|BMaABIVolQs69${1Vn3^$QQcEvJ~Nb+dFXRy#xW(a*j)(UK=|-d zcn6q4!X+ieTYTFoZcgp#=XXZ9;UUc;T=%K`eBug-jIp$soK(Z1fMXS>VvE~It{8>b za-G0wD$2mPenz?TQJ614j>{nB zm!`rqNz|oy=e2p)lxD^?n*atsLRfUiz|xc5aO}fJdk9+LDj)#qB|0sI{;`?K@ca#G z*d1;}%Ec(V&*xlMV#H)Y-q^-I+ZEo@0nED&>vl9n|HuHJy}R0#$18pTk3A*}9vuO+ z3g_~uL+av28#38siXfrtRV^Or^-SxYLU{e~$?s<-9h + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loaderHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-09 22:03:33Functions: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..3a30186c2d --- /dev/null +++ b/mrs_lib/src/param_loader/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loaderHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-09 22:03:33Functions: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..52453421e0 --- /dev/null +++ b/mrs_lib/src/param_loader/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loaderHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-09 22:03:33Functions: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..95b4419076 --- /dev/null +++ b/mrs_lib/src/param_loader/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loaderHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-09 22:03:33Functions: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..42a97b0515 --- /dev/null +++ b/mrs_lib/src/param_loader/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loaderHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-09 22:03:33Functions: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..b3bad10a6a --- /dev/null +++ b/mrs_lib/src/param_loader/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loaderHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-09 22:03:33Functions: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..3899743760 --- /dev/null +++ b/mrs_lib/src/param_loader/param_provider.cpp.func-sort-c.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader/param_provider.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loader - param_provider.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-09 22:03:33Functions: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)3558
mrs_lib::ParamProvider::addYamlFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)11675
mrs_lib::ParamProvider::findYamlNode(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) const78047
+
+
+ + + +
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..b4c2a8244f --- /dev/null +++ b/mrs_lib/src/param_loader/param_provider.cpp.func.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader/param_provider.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loader - param_provider.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-09 22:03:33Functions: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&)11675
mrs_lib::ParamProvider::ParamProvider(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool)3558
mrs_lib::ParamProvider::findYamlNode(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) const78047
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..3b1dc0b0c2 --- /dev/null +++ b/mrs_lib/src/param_loader/param_provider.cpp.gcov.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader/param_provider.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/param_loader - param_provider.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:445580.0 %
Date:2024-04-09 22:03:33Functions: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        3558 :   ParamProvider::ParamProvider(const ros::NodeHandle& nh, std::string node_name, const bool use_rosparam)
+      14        3558 :   : m_nh(nh), m_node_name(std::move(node_name)), m_use_rosparam(use_rosparam)
+      15             :   {
+      16        3558 :   }
+      17             : 
+      18       11675 :   bool ParamProvider::addYamlFile(const std::string& filepath)
+      19             :   {
+      20             :     try
+      21             :     {
+      22       23350 :       const auto loaded_yaml = YAML::LoadFile(filepath);
+      23       11675 :       YAML::Node root;
+      24       11675 :       root["root"] = loaded_yaml;
+      25       11675 :       m_yamls.emplace_back(root);
+      26       11675 :       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       78047 :   std::optional<YAML::Node> ParamProvider::findYamlNode(const std::string& param_name) const
+      65             :   {
+      66      404156 :     for (const auto& yaml : m_yamls)
+      67             :     {
+      68             :       // Try to load the parameter sequentially as a map.
+      69      390303 :       auto cur_node_it = std::cbegin(yaml);
+      70             :       // The root should always be a pam
+      71      390302 :       if (!cur_node_it->second.IsMap())
+      72       12151 :         continue;
+      73             : 
+      74      378150 :       bool loaded = true;
+      75             :       {
+      76      378150 :         constexpr char delimiter = '/';
+      77      378150 :         auto substr_start = std::cbegin(param_name);
+      78      378151 :         auto substr_end = substr_start;
+      79      516349 :         do
+      80             :         {
+      81      894500 :           substr_end = std::find(substr_start, std::cend(param_name), delimiter);
+      82             :           // why can't substr or string_view take iterators? :'(
+      83      894502 :           const auto start_pos = std::distance(std::cbegin(param_name), substr_start);
+      84      894493 :           const auto count = std::distance(substr_start, substr_end);
+      85      894494 :           const std::string param_substr = param_name.substr(start_pos, count);
+      86      894494 :           substr_start = substr_end+1;
+      87             : 
+      88      894495 :           bool found = false;
+      89     3661602 :           for (auto node_it = std::cbegin(cur_node_it->second); node_it != std::cend(cur_node_it->second); ++node_it)
+      90             :           {
+      91     1872594 :             if (node_it->first.as<std::string>() == param_substr)
+      92             :             {
+      93      580549 :               cur_node_it = node_it;
+      94      580549 :               found = true;
+      95      580549 :               break;
+      96             :             }
+      97             :           }
+      98             : 
+      99      894503 :           if (!found)
+     100             :           {
+     101      313957 :             loaded = false;
+     102      313957 :             break;
+     103             :           }
+     104             :         }
+     105      580545 :         while (substr_end != std::end(param_name) && cur_node_it->second.IsMap());
+     106             :       }
+     107             : 
+     108      378151 :       if (loaded)
+     109             :       {
+     110      128386 :         return cur_node_it->second;
+     111             :       }
+     112             :     }
+     113             : 
+     114       13852 :     return std::nullopt;
+     115             :   }
+     116             : }
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/param_loader/param_provider.cpp.gcov.overview.html b/mrs_lib/src/param_loader/param_provider.cpp.gcov.overview.html new file mode 100644 index 0000000000..af67093666 --- /dev/null +++ b/mrs_lib/src/param_loader/param_provider.cpp.gcov.overview.html @@ -0,0 +1,49 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/param_loader/param_provider.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

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

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

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

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

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
profiler.cpp +
37.6%37.6%
+ 		37.6 %38 / 10190.0 %9 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/profiler/profiler.cpp.func-sort-c.html b/mrs_lib/src/profiler/profiler.cpp.func-sort-c.html new file mode 100644 index 0000000000..a227ea94d9 --- /dev/null +++ b/mrs_lib/src/profiler/profiler.cpp.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler/profiler.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profiler - profiler.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3810137.6 %
Date:2024-04-09 22:03:33Functions: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)1149
mrs_lib::Profiler::Profiler()1149
mrs_lib::Profiler::operator=(mrs_lib::Profiler const&)1149
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, double, ros::TimerEvent)598970
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)599083
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)1726455
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)1726492
mrs_lib::Routine::end()2325427
mrs_lib::Routine::~Routine()2325551
+
+
+ + + +
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..bba38497b2 --- /dev/null +++ b/mrs_lib/src/profiler/profiler.cpp.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler/profiler.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profiler - profiler.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3810137.6 %
Date:2024-04-09 22:03:33Functions:91090.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_lib::Routine::end()2325427
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)1726455
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)599083
mrs_lib::Routine::~Routine()2325551
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)1726492
mrs_lib::Profiler::createRoutine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, double, ros::TimerEvent)598970
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)1149
mrs_lib::Profiler::Profiler()1149
mrs_lib::Profiler::operator=(mrs_lib::Profiler const&)1149
+
+
+ + + +
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..af6079080e --- /dev/null +++ b/mrs_lib/src/profiler/profiler.cpp.gcov.html @@ -0,0 +1,301 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler/profiler.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/profiler - profiler.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3810137.6 %
Date:2024-04-09 22:03:33Functions: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        1149 : Profiler::Profiler() {
+      11        1149 : }
+      12             : 
+      13        1149 : Profiler::Profiler(ros::NodeHandle& nh, std::string _node_name_, bool profiler_enabled) {
+      14             : 
+      15        1149 :   this->nh_                = std::make_shared<ros::NodeHandle>(nh);
+      16        1149 :   this->_node_name_        = _node_name_;
+      17        1149 :   this->_profiler_enabled_ = profiler_enabled;
+      18             : 
+      19        1149 :   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        1149 :   ROS_INFO("[%s]: profiler initialized", _node_name_.c_str());
+      25             : 
+      26        1149 :   this->is_initialized_ = true;
+      27        1149 : }
+      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        1149 : Profiler& Profiler::operator=(const Profiler& other) {
+      43             : 
+      44        1149 :   if (this == &other) {
+      45           0 :     return *this;
+      46             :   }
+      47             : 
+      48        1149 :   this->is_initialized_    = other.is_initialized_;
+      49        1149 :   this->nh_                = other.nh_;
+      50        1149 :   this->_node_name_        = other._node_name_;
+      51        1149 :   this->_profiler_enabled_ = other._profiler_enabled_;
+      52             : 
+      53        1149 :   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        1149 :   return *this;
+      59             : }
+      60             : 
+      61             : //}
+      62             : 
+      63             : /* Profiler::registerRoutine() for periodic //{ */
+      64             : 
+      65      598970 : Routine Profiler::createRoutine(std::string name, double expected_rate, double threshold, ros::TimerEvent event) {
+      66             : 
+      67      598970 :   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     1726492 : Routine Profiler::createRoutine(std::string name) {
+      75             : 
+      76     1726492 :   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      599083 : Routine::Routine(std::string name, std::string node_name, double expected_rate, double threshold, std::shared_ptr<ros::Publisher> publisher,
+      86      599083 :                  std::shared_ptr<std::mutex> mutex_publisher, bool profiler_enabled, ros::TimerEvent event) {
+      87             : 
+      88      598149 :   if (!profiler_enabled) {
+      89      598231 :     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     1726455 : Routine::Routine(std::string name, std::string node_name, std::shared_ptr<ros::Publisher> publisher, std::shared_ptr<std::mutex> mutex_publisher,
+     141     1726455 :                  bool profiler_enabled) {
+     142             : 
+     143     1725489 :   if (!profiler_enabled) {
+     144     1725503 :     return;
+     145             :   }
+     146             : 
+     147           3 :   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     2325427 : void Routine::end(void) {
+     186             : 
+     187     2325427 :   if (!_profiler_enabled_) {
+     188     2325094 :     return;
+     189             :   }
+     190             : 
+     191         333 :   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     2325309 : Routine::~Routine() {
+     213             : 
+     214     2325551 :   this->end();
+     215     2323664 : }
+     216             : 
+     217             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/profiler/profiler.cpp.gcov.overview.html b/mrs_lib/src/profiler/profiler.cpp.gcov.overview.html new file mode 100644 index 0000000000..f66727ff8b --- /dev/null +++ b/mrs_lib/src/profiler/profiler.cpp.gcov.overview.html @@ -0,0 +1,75 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/profiler/profiler.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/profiler/profiler.cpp.gcov.png b/mrs_lib/src/profiler/profiler.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..9724b44a6103493cc24901baf8b7a598ae3b23c7 GIT binary patch literal 829 zcmV-D1H$}?P)jX4>?fp(O*C8=7vv$G0st@QIgJxr;(ykZ$z$lG3!tG4g=aZ^cF{d z^}aVvg#$Pj6yd${+5L}WQr79RxG5%sjugjnif}L}vb9PU9VvIAj9upw z=yEW`igkF0TIBsQuHj&-iL2DBh#KJ$n(C&&%Sr&4%-Q|F1Asl42_%@G-Pax#F_2~> z<`9<+ibY&b`3P|f)V+kb-X<<5F5+VlcM@zaNood zPWc5;lzsCWG)wD`)#9uGy-iU$EA;6JSnpif2l6(ol*G*zk?c_t7duTWM!GnGS`g|g zD^|O9Qq08lnPRnT+S}V$^d(fe2G3kT*Mf1FglDFa;(F8_saS(TePSov^v);S{NR;62%sgr)|yf^&jikY3Ij-tf~Pa-tdm(YnG@19V_b8|#JmS9GS5hB zqi#nP-lln?_%=<+V(+_UL%Xx~t(2}E + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-04-09 22:03:33Functions: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..fc9908cd96 --- /dev/null +++ b/mrs_lib/src/safety_zone/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-04-09 22:03:33Functions: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..47cebc417d --- /dev/null +++ b/mrs_lib/src/safety_zone/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-04-09 22:03:33Functions: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..4daa7a1451 --- /dev/null +++ b/mrs_lib/src/safety_zone/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-04-09 22:03:33Functions: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..482ae10cee --- /dev/null +++ b/mrs_lib/src/safety_zone/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-04-09 22:03:33Functions: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..b82ca6543d --- /dev/null +++ b/mrs_lib/src/safety_zone/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zoneHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:314963.3 %
Date:2024-04-09 22:03:33Functions: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..09a24cf3c9 --- /dev/null +++ b/mrs_lib/src/safety_zone/line_operations.cpp.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/line_operations.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - line_operations.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:162466.7 %
Date:2024-04-09 22:03:33Functions: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>)218
mrs_lib::Intersection::Intersection(bool, bool, Eigen::Matrix<double, 1, 2, 1, 1, 2>)248
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>)248
+
+
+ + + +
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..568e2f2115 --- /dev/null +++ b/mrs_lib/src/safety_zone/line_operations.cpp.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/line_operations.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - line_operations.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:162466.7 %
Date:2024-04-09 22:03:33Functions: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>)218
mrs_lib::Intersection::Intersection(bool, bool, Eigen::Matrix<double, 1, 2, 1, 1, 2>)248
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>)248
+
+
+ + + +
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..5989edb448 --- /dev/null +++ b/mrs_lib/src/safety_zone/line_operations.cpp.gcov.html @@ -0,0 +1,143 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/line_operations.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - line_operations.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:162466.7 %
Date:2024-04-09 22:03:33Functions: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         218 : 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         218 :   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         248 : Intersection::Intersection(bool intersect, bool parallel, Eigen::RowVector2d point) : point(std::move(point)), parallel(parallel), intersect(intersect){}
+      16             : 
+      17             : /* sectionIntersect() //{ */
+      18             : 
+      19         248 : Intersection sectionIntersect(Eigen::RowVector2d start1, Eigen::RowVector2d end1, Eigen::RowVector2d start2, Eigen::RowVector2d end2) {
+      20         248 :   Eigen::RowVector2d vector1 = end1 - start1;
+      21         248 :   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         248 :   double cross            = vector1(0) * vector2(1) - vector1(1) * vector2(0);
+      29         248 :   double difference_cross = (start2(0) - start1(0)) * vector2(1) - (start2(1) - start1(1)) * vector2(0);
+      30             : 
+      31         248 :   if (cross == 0) {
+      32             :     // are parallel
+      33          30 :     if (difference_cross != 0)
+      34          30 :       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         218 :   double             scale1 = difference_cross / cross;
+      48         218 :   Eigen::RowVector2d point  = start1 + scale1 * vector1;
+      49         218 :   double             scale2 = getScale(start2, vector2, point);
+      50             : 
+      51         218 :   if (0 <= scale1 && scale1 <= 1 && 0 <= scale2 && scale2 <= 1) {
+      52           0 :     return Intersection{true, false, point};
+      53             :   }
+      54         218 :   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..280486e1a0 --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-09 22:03:33Functions: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..5fcf774300 --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-09 22:03:33Functions: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..85af5d6800 --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-09 22:03:33Functions: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..98867716da --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-09 22:03:33Functions: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..ba83905590 --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-09 22:03:33Functions: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..d22a60ecea --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygonHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-09 22:03:33Functions: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..5a0a53282f --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/polygon.cpp.func-sort-c.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon/polygon.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygon - polygon.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-09 22:03:33Functions: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::doesSectionIntersect(double, double, double, double)62
mrs_lib::Polygon::Polygon(Eigen::Matrix<double, -1, -1, 0, -1, -1>)62
mrs_lib::Polygon::isPointInside(double, double)9737
mrs_lib::Polygon::getPointMessageVector(double)20186
+
+
+ + + +
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..2984c23a6d --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/polygon.cpp.func.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon/polygon.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygon - polygon.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-09 22:03:33Functions: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)9737
mrs_lib::Polygon::doesSectionIntersect(double, double, double, double)62
mrs_lib::Polygon::getPointMessageVector(double)20186
mrs_lib::Polygon::Polygon(Eigen::Matrix<double, -1, -1, 0, -1, -1>)62
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..d43bcf3bd6 --- /dev/null +++ b/mrs_lib/src/safety_zone/polygon/polygon.cpp.gcov.html @@ -0,0 +1,290 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/polygon/polygon.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone/polygon - polygon.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4210340.8 %
Date:2024-04-09 22:03:33Functions: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          62 : Polygon::Polygon(const Eigen::MatrixXd vertices) : vertices(vertices) {
+      10             : 
+      11          62 :   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          62 :   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          62 :   Eigen::RowVector2d edge1;
+      21          62 :   Eigen::RowVector2d edge2 = vertices.row(1) - vertices.row(0);
+      22         310 :   for (int i = 0; i < vertices.rows(); ++i) {
+      23         248 :     edge1 = edge2;
+      24         248 :     edge2 = vertices.row((i + 2) % vertices.rows()) - vertices.row((i + 1) % vertices.rows());
+      25             : 
+      26         248 :     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         248 :     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          62 : }
+      36             : 
+      37             : //}
+      38             : 
+      39             : /* isPointInside() //{ */
+      40             : 
+      41        9737 : bool Polygon::isPointInside(const double px, const double py) {
+      42             : 
+      43        9737 :   int count = 0;
+      44             : 
+      45             :   // Cast a horizontal ray and see how many times it intersects all the edges
+      46       48685 :   for (int i = 0; i < vertices.rows(); ++i) {
+      47       38948 :     double v1x = vertices(i, 0);
+      48       38948 :     double v1y = vertices(i, 1);
+      49       38948 :     double v2x = vertices((i + 1) % vertices.rows(), 0);
+      50       38948 :     double v2y = vertices((i + 1) % vertices.rows(), 1);
+      51             : 
+      52       38948 :     if (v1y > py && v2y > py)
+      53        9659 :       continue;
+      54       29289 :     if (v1y <= py && v2y <= py)
+      55        9971 :       continue;
+      56       19318 :     double intersect_x    = (v1y == v2y) ? v1x : (py - v1y) * (v2x - v1x) / (v2y - v1y) + v1x;
+      57       19318 :     bool   does_intersect = intersect_x > px;
+      58       19318 :     if (does_intersect)
+      59        9659 :       ++count;
+      60             :   }
+      61             : 
+      62        9737 :   return count % 2;
+      63             : }
+      64             : 
+      65             : //}
+      66             : 
+      67             : /* doesSectionIntersect() //{ */
+      68             : 
+      69          62 : bool Polygon::doesSectionIntersect(const double startX, const double startY, const double endX, const double endY) {
+      70             : 
+      71          62 :   Eigen::RowVector2d start{startX, startY};
+      72          62 :   Eigen::RowVector2d end{endX, endY};
+      73             : 
+      74         310 :   for (int i = 0; i < vertices.rows(); ++i) {
+      75             : 
+      76         248 :     Eigen::RowVector2d edgeStart = vertices.row(i);
+      77         248 :     Eigen::RowVector2d edgeEnd   = vertices.row((i + 1) % vertices.rows());
+      78             : 
+      79         248 :     if (sectionIntersect(start, end, edgeStart, edgeEnd).intersect) {
+      80           0 :       return true;
+      81             :     }
+      82             :   }
+      83             : 
+      84          62 :   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       20186 : std::vector<geometry_msgs::Point> Polygon::getPointMessageVector(const double z) {
+     194       20186 :   std::vector<geometry_msgs::Point> points(vertices.rows());
+     195      100930 :   for (int i = 0; i < vertices.rows(); ++i) {
+     196       80744 :     points[i].x = vertices(i, 0);
+     197       80744 :     points[i].y = vertices(i, 1);
+     198       80744 :     points[i].z = z;
+     199             :   }
+     200             : 
+     201       20186 :   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..84675d6704 --- /dev/null +++ b/mrs_lib/src/safety_zone/safety_zone.cpp.func-sort-c.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/safety_zone.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - safety_zone.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:152560.0 %
Date:2024-04-09 22:03:33Functions: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::isPathValid(double, double, double, double)62
mrs_lib::SafetyZone::SafetyZone(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)62
mrs_lib::SafetyZone::~SafetyZone()62
mrs_lib::SafetyZone::isPointValid(double, double)9737
mrs_lib::SafetyZone::getBorder()10093
+
+
+ + + +
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..50186feb6c --- /dev/null +++ b/mrs_lib/src/safety_zone/safety_zone.cpp.func.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/safety_zone.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - safety_zone.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:152560.0 %
Date:2024-04-09 22:03:33Functions: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)62
mrs_lib::SafetyZone::isPointValid(double, double)9737
mrs_lib::SafetyZone::getBorder()10093
mrs_lib::SafetyZone::SafetyZone(mrs_lib::Polygon)0
mrs_lib::SafetyZone::SafetyZone(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)62
mrs_lib::SafetyZone::~SafetyZone()62
+
+
+ + + +
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..00c033e0af --- /dev/null +++ b/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.html @@ -0,0 +1,160 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/safety_zone.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/safety_zone - safety_zone.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:152560.0 %
Date:2024-04-09 22:03:33Functions: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         124 : SafetyZone::~SafetyZone() {
+      17          62 :   delete outerBorder;
+      18          62 : }
+      19             : 
+      20             : //}
+      21             : 
+      22             : /* SafetyZone() //{ */
+      23             : 
+      24          62 : SafetyZone::SafetyZone(const Eigen::MatrixXd& outerBorderMatrix) {
+      25             : 
+      26             :   try {
+      27          62 :     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          62 : }
+      39             : 
+      40             : //}
+      41             : 
+      42             : /* isPointValid() //{ */
+      43             : 
+      44        9737 : bool SafetyZone::isPointValid(const double px, const double py) {
+      45             : 
+      46        9737 :   if (!outerBorder->isPointInside(px, py)) {
+      47          78 :     return false;
+      48             :   }
+      49             : 
+      50        9659 :   return true;
+      51             : }
+      52             : 
+      53             : //}
+      54             : 
+      55             : /* isPathValid() //{ */
+      56             : 
+      57          62 : bool SafetyZone::isPathValid(const double p1x, const double p1y, const double p2x, const double p2y) {
+      58             : 
+      59          62 :   if (outerBorder->doesSectionIntersect(p1x, p1y, p2x, p2y)) {
+      60           0 :     return false;
+      61             :   }
+      62             : 
+      63          62 :   return true;
+      64             : }
+      65             : 
+      66             : //}
+      67             : 
+      68             : /* getBorder() //{ */
+      69             : 
+      70       10093 : Polygon SafetyZone::getBorder() {
+      71       10093 :   return *outerBorder;
+      72             : }
+      73             : 
+      74             : //}
+      75             : 
+      76             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.overview.html b/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.overview.html new file mode 100644 index 0000000000..516836160f --- /dev/null +++ b/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.overview.html @@ -0,0 +1,39 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/safety_zone/safety_zone.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.png b/mrs_lib/src/safety_zone/safety_zone.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..167b4f7e497806e1c5538d1ea552833672a06ad0 GIT binary patch literal 380 zcmeAS@N?(olHy`uVBq!ia0vp^0YL1-!VDxiv~}x%lth3}i0l9V|5pLQ^7pZ^ul_SI ztOAOIsdL_&mjcUg-y18yaI7 z%{TmBAEdP6g!??PYWMlaW!d{%T5MX_xDAgi5UEU>#?=(=ooAXfbwgx{^ptPA_pfEX zr8Q=~Zx;(p%CC$O zP7U7LAJccMt@r5KDfW90#J-#3(dd47R^nEljR%4XE7Kywv;{-LPkx;e_>ezFUC{o? zF6FZ(B4^i4cF>!}Zu))uYmWY?qhdFgoD=+88s=@YPEz7t@m-}D5rcW!T!tNP+a@o% z@J`_IIdj*CXR4B|CzhW5w0@=7b<>`Dovt!d$M*P_C!JL*md<&;soTa&apCeG>;_zJ VEeW!JlYl|R;OXk;vd$@?2>{; + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-09 22:03:33Functions: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..1e8b7193b3 --- /dev/null +++ b/mrs_lib/src/scope_timer/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-09 22:03:33Functions: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..35be51d087 --- /dev/null +++ b/mrs_lib/src/scope_timer/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-09 22:03:33Functions: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..5f7cf43f8d --- /dev/null +++ b/mrs_lib/src/scope_timer/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-09 22:03:33Functions: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..1534aab546 --- /dev/null +++ b/mrs_lib/src/scope_timer/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-09 22:03:33Functions: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..a00ecfc47e --- /dev/null +++ b/mrs_lib/src/scope_timer/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-09 22:03:33Functions: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..382a4ea46c --- /dev/null +++ b/mrs_lib/src/scope_timer/scope_timer.cpp.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer/scope_timer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timer - scope_timer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-09 22:03:33Functions: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)574
mrs_lib::ScopeTimerLogger::~ScopeTimerLogger()574
mrs_lib::ScopeTimer::checkpoint(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3706938
mrs_lib::ScopeTimer::ScopeTimer(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<mrs_lib::ScopeTimerLogger>, bool)4790159
mrs_lib::ScopeTimer::~ScopeTimer()4792119
+
+
+ + + +
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..d0eba32f64 --- /dev/null +++ b/mrs_lib/src/scope_timer/scope_timer.cpp.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer/scope_timer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timer - scope_timer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-09 22:03:33Functions: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&)3706938
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)4790159
mrs_lib::ScopeTimer::~ScopeTimer()4792119
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)574
mrs_lib::ScopeTimerLogger::~ScopeTimerLogger()574
+
+
+ + + +
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..df58426cfb --- /dev/null +++ b/mrs_lib/src/scope_timer/scope_timer.cpp.gcov.html @@ -0,0 +1,345 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer/scope_timer.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/scope_timer - scope_timer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2014214.1 %
Date:2024-04-09 22:03:33Functions: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         574 : ScopeTimerLogger::ScopeTimerLogger(const std::string& logfile, const bool enable_logging, const int log_precision)
+      10         574 :     : _logging_enabled_(enable_logging), _log_filepath_(logfile) {
+      11             : 
+      12         574 :   if (!_logging_enabled_) {
+      13         574 :     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         574 : ScopeTimerLogger::~ScopeTimerLogger() {
+      47         574 :   if (_logging_enabled_) {
+      48           0 :     ROS_DEBUG("[%s]: ScopeTimerLogger: closing logstream.", ros::this_node::getName().c_str());
+      49           0 :     _logstream_.close();
+      50             :   }
+      51         574 : }
+      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     4790159 : ScopeTimer::ScopeTimer(const std::string& label, const std::shared_ptr<ScopeTimerLogger> scope_timer_logger, const bool enable)
+     104     4790159 :     : _timer_label_(label), _enable_print_or_log(enable), _throttle_period_(ros::Duration(0.0)), _logger_(scope_timer_logger) {
+     105             : 
+     106     4780619 :   checkpoints.push_back(time_point("timer start"));
+     107             : 
+     108     4778730 :   ROS_DEBUG("[%s] Scope timer started with file logger (label: %s).", ros::this_node::getName().c_str(), label.c_str());
+     109     4778730 : }
+     110             : 
+     111             : //}
+     112             : 
+     113             : /* ScopeTimer::checkpoint() //{ */
+     114             : 
+     115     3706938 : void ScopeTimer::checkpoint(const std::string& label) {
+     116             : 
+     117     3706938 :   if (_enable_print_or_log) {
+     118           0 :     checkpoints.push_back(time_point(label));
+     119             :   }
+     120     3706938 : }
+     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    14370080 : ScopeTimer::~ScopeTimer() {
+     136             : 
+     137     4792119 :   if (!_enable_print_or_log) {
+     138     4792159 :     return;
+     139             :   }
+     140             : 
+     141          15 :   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     4789507 : }
+     258             : 
+     259             : //}
+     260             : 
+     261             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/scope_timer/scope_timer.cpp.gcov.overview.html b/mrs_lib/src/scope_timer/scope_timer.cpp.gcov.overview.html new file mode 100644 index 0000000000..8fad4dc2df --- /dev/null +++ b/mrs_lib/src/scope_timer/scope_timer.cpp.gcov.overview.html @@ -0,0 +1,86 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/scope_timer/scope_timer.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/scope_timer/scope_timer.cpp.gcov.png b/mrs_lib/src/scope_timer/scope_timer.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..1da9040a91ee22c35f65971da31ab5417532af9e GIT binary patch literal 1280 zcmV+b1^@bqP)#&hXlvB)tz2;Yvu&Pmt{=i}_=T zTK4#!0jGfVp*J=aI^6T6eR)Xs@1Sq%sQA^8*Zq0o;p;v!$8mOmczl4ELn-E9|9_q% z;1LlDjn-e63vUqbZAXl?85BNxI0keVJ8+l3d^(Fq0PGIPH7Y;=#PQs~KD1G((P#rH zyC+S%IXnH(sL^|-FX?7imoy6FdTxyxRg2Amj$R%KzqZez+g$$p^_^5`D?+93qK zYs^|5W=fn@*a${SjlzDJ>v}(O3Lo*smkGcN|D;nV{MH9rEwIw46n#m$u(6|Ih;yEA zCGpV7px_F9BYgEMqbA1(h?LjinB(Hg8+EvlTIKSs3sF3L4!B1xG8P-@pGYc8V+YI^ zV%<{*cjI)S+A9YmO}+%KYRnsZDmsKb!nhNMJ22_rarme#{0l2X4+gT-+F+5~!?Fj=DGlO=k&Q)p*h?zF#z-_y$Y$m6UquPZKlIUZT+t@LsW`v}~`k zD1NV@G3scDVtABf4-~`d$vnJ-;iT7+(o4a0gB}25c31Hz=f_LH`W+-S_vqd8ki++n zxm(^qPFi-_cx*Nin;LcZapq9wf_~NqGAG(7X(LF|$ivLDvcHZ;b@YT~!~QK+j$2sJ z_`XT-z7W4Lwk!`^M`Pq3d)E`wc37H?ElLW(R(B`P<1GTb9Oh-M5&ETWY$tW%RISez zkydVp0;%H?q0ykrH`-?~fo{)7zf%^`&@`8{+Ksv=-9=Sk##whzL@&D3UnYM->4Q}v z6mc}aQO_;`X{~#V7J({4GoOF@k-Re-+N!&*AMXKR7H$v=8=c*8vcvHmDwKte=SwT< z76<@X9CLm{8{){j7`^O=cNyTD-RpZY8{G->9vUz13~O^^vJ7m5L;cWZY|kZ03e%NVQy6W<(y4SQ{QVn9OHUEp(DQEr zC*A868)>Seu^Di#5qW1EZmWJoN*lOnNh8Hv$k;j1(YDpwx+x27+zA03mZ;S@x3q@r zX=7KTW2k|RVs{-)=7Gn6c@_$OgwS=K&0W?*P4`^9s_~gAgnN3?#@*9fltuS&eUs?X zNzNJH*tqlMs{)qTc)6*u1*B)1AJkZM3#YSI2ZdInO)X3wHJ3c~Da_#hqkoH3XcUi7 z96l<3*GA#H!orI-_L7wWZd*WUJWi(s>TN6+`>IbJ0>jlw>MUtowK2zGp)5Tb`X2C! zA70b}UiXrWGQh1-2Kf4Z&7}%(@0pEU;9L2u(kKIb@n<`&a4bp9D3{u4fohG{3UFa% qP!GN+fTR!puZHgqO{?jTDt`k-lcJw@c_}ym0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-09 22:03:33Functions: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..1819ff4792 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-09 22:03:33Functions: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..6350334904 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-09 22:03:33Functions: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..4569718767 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-09 22:03:33Functions: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..dec7aa5f36 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-09 22:03:33Functions: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..8bfe335442 --- /dev/null +++ b/mrs_lib/src/timeout_manager/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-09 22:03:33Functions: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..d037c18ab1 --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.func-sort-c.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_manager - timeout_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-09 22:03:33Functions: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&)78
mrs_lib::TimeoutManager::registerNew(ros::Duration const&, std::function<void (ros::Time const&)> const&, ros::Time const&, bool, bool)81
mrs_lib::TimeoutManager::start(unsigned long, ros::Time const&)81
mrs_lib::TimeoutManager::started(unsigned long)1590
mrs_lib::TimeoutManager::main_timer_callback(ros::TimerEvent const&)19951
mrs_lib::TimeoutManager::reset(unsigned long, ros::Time const&)173629
+
+
+ + + +
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..50a85b1422 --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.func.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_manager - timeout_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-09 22:03:33Functions: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)81
mrs_lib::TimeoutManager::main_timer_callback(ros::TimerEvent const&)19951
mrs_lib::TimeoutManager::pause(unsigned long)9
mrs_lib::TimeoutManager::reset(unsigned long, ros::Time const&)173629
mrs_lib::TimeoutManager::start(unsigned long, ros::Time const&)81
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)1590
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&)78
+
+
+ + + +
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..14ed54adb5 --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.html @@ -0,0 +1,186 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timeout_manager - timeout_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:496279.0 %
Date:2024-04-09 22:03:33Functions: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          78 :   TimeoutManager::TimeoutManager(const ros::NodeHandle& nh, const ros::Rate& update_rate)
+       7          78 :     : m_last_id(0)
+       8             :   {
+       9          78 :     m_main_timer = nh.createTimer(update_rate, &TimeoutManager::main_timer_callback, this);
+      10          78 :   }
+      11             : 
+      12             : 
+      13          81 :   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         162 :     std::scoped_lock lck(m_mtx);
+      16          81 :     const auto new_id = m_timeouts.size();
+      17          81 :     const timeout_info_t new_info = {oneshot, autostart, callback, timeout, last_reset, last_reset};
+      18          81 :     m_timeouts.push_back(new_info);
+      19         162 :     return new_id;
+      20             :   }
+      21             : 
+      22      173629 :   void TimeoutManager::reset(const timeout_id_t id, const ros::Time& time)
+      23             :   {
+      24      173629 :     std::scoped_lock lck(m_mtx);
+      25      173629 :     m_timeouts.at(id).last_reset = time;
+      26      173629 :   }
+      27             : 
+      28           9 :   void TimeoutManager::pause(const timeout_id_t id)
+      29             :   {
+      30           9 :     std::scoped_lock lck(m_mtx);
+      31           9 :     m_timeouts.at(id).started = false;
+      32           9 :   }
+      33             : 
+      34          81 :   void TimeoutManager::start(const timeout_id_t id, const ros::Time& time)
+      35             :   {
+      36          81 :     std::scoped_lock lck(m_mtx);
+      37          81 :     m_timeouts.at(id).started = true;
+      38          81 :     m_timeouts.at(id).last_reset = time;
+      39          81 :   }
+      40             : 
+      41           2 :   void TimeoutManager::pauseAll()
+      42             :   {
+      43           4 :     std::scoped_lock lck(m_mtx);
+      44          10 :     for (auto& timeout_info : m_timeouts)
+      45           8 :       timeout_info.started = false;
+      46           2 :   }
+      47             : 
+      48           4 :   void TimeoutManager::startAll(const ros::Time& time)
+      49             :   {
+      50           8 :     std::scoped_lock lck(m_mtx);
+      51          20 :     for (auto& timeout_info : m_timeouts)
+      52             :     {
+      53          16 :       timeout_info.started = true;
+      54          16 :       timeout_info.last_reset = time;
+      55             :     }
+      56           4 :   }
+      57             : 
+      58           0 :   void TimeoutManager::change(const timeout_id_t id, const ros::Duration& timeout, const callback_t& callback, const ros::Time& last_reset, const bool oneshot, const bool autostart)
+      59             :   {
+      60           0 :     std::scoped_lock lck(m_mtx);
+      61           0 :     auto& timeout_info = m_timeouts.at(id);
+      62           0 :     timeout_info.oneshot = oneshot;
+      63           0 :     timeout_info.started = autostart;
+      64           0 :     timeout_info.timeout = timeout;
+      65           0 :     timeout_info.callback = callback;
+      66           0 :     timeout_info.last_reset = last_reset;
+      67           0 :   }
+      68             : 
+      69           0 :   ros::Time TimeoutManager::lastReset(const timeout_id_t id)
+      70             :   {
+      71           0 :     std::scoped_lock lck(m_mtx);
+      72           0 :     return m_timeouts.at(id).last_reset;
+      73             :   }
+      74             : 
+      75        1590 :   bool TimeoutManager::started(const timeout_id_t id)
+      76             :   {
+      77        1590 :     std::scoped_lock lck(m_mtx);
+      78        3180 :     return m_timeouts.at(id).started;
+      79             :   }
+      80             : 
+      81       19951 :   void TimeoutManager::main_timer_callback([[maybe_unused]] const ros::TimerEvent &evt)
+      82             :   {
+      83       19951 :     const auto now = ros::Time::now();
+      84             : 
+      85       39902 :     std::scoped_lock lck(m_mtx);
+      86       46184 :     for (auto& timeout_info : m_timeouts)
+      87             :     {
+      88             :       // don't worry, this'll get optimized out by the compiler
+      89       26233 :       const bool started = timeout_info.started;
+      90       26233 :       const bool last_reset_timeout = now - timeout_info.last_reset >= timeout_info.timeout;
+      91       26233 :       const bool last_callback_timeout = now - timeout_info.last_callback >= timeout_info.timeout;
+      92       26233 :       if (started && last_reset_timeout && last_callback_timeout)
+      93             :       {
+      94        2110 :         timeout_info.callback(timeout_info.last_reset);
+      95        2110 :         timeout_info.last_callback = now;
+      96             :         // if the timeout is oneshot, pause it
+      97        2110 :         if (timeout_info.oneshot)
+      98           0 :           timeout_info.started = false;
+      99             :       }
+     100             :     }
+     101       19951 :   }
+     102             : }
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html new file mode 100644 index 0000000000..0b582c4d4a --- /dev/null +++ b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.overview.html @@ -0,0 +1,46 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timeout_manager/timeout_manager.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.png b/mrs_lib/src/timeout_manager/timeout_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..c867c6a7540f7d57f11eda65b1d826deefd78012 GIT binary patch literal 512 zcmV+b0{{JqP)I*~y25zOg^hr1xiNM+LG`58W&G$N9I|OP#X`T26 zz@*c|b>>k1w4ja|9@(hw%rz(Ay>h5(ab;1n0$wqzKKOxp#J!%Up7I)M2LZn5^~iOmac@97IKq74SZ?s&eJvH{0Jg=d3&8 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/timer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/timerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10511492.1 %
Date:2024-04-09 22:03:33Functions:1515100.0 %
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 +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
<unnamed>92.1 %105 / 114100.0 %15 / 15
+
+
+ + + + +
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..c2a4b6bbb9 --- /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:10511492.1 %
Date:2024-04-09 22:03:33Functions:1515100.0 %
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 +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
<unnamed>92.1 %105 / 114100.0 %15 / 15
+
+
+ + + + +
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..bafe8206be --- /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:10511492.1 %
Date:2024-04-09 22:03:33Functions:1515100.0 %
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 +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
<unnamed>92.1 %105 / 114100.0 %15 / 15
+
+
+ + + + +
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..62b1dc4ec2 --- /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:10511492.1 %
Date:2024-04-09 22:03:33Functions:1515100.0 %
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 +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
+
+
+ + + + +
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..72889b1e0e --- /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:10511492.1 %
Date:2024-04-09 22:03:33Functions:1515100.0 %
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 +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
+
+
+ + + + +
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..fcd75ef27e --- /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:10511492.1 %
Date:2024-04-09 22:03:33Functions:1515100.0 %
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 +
92.1%92.1%
+ 		92.1 %105 / 114100.0 %15 / 15
+
+
+ + + + +
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..0eef859c5e --- /dev/null +++ b/mrs_lib/src/timer/timer.cpp.func-sort-c.html @@ -0,0 +1,140 @@ + + + + + + + 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:10511492.1 %
Date:2024-04-09 22:03:33Functions:1515100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
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..310e10ecde --- /dev/null +++ b/mrs_lib/src/timer/timer.cpp.func.html @@ -0,0 +1,140 @@ + + + + + + + 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:10511492.1 %
Date:2024-04-09 22:03:33Functions:1515100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
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::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..4abcafe067 --- /dev/null +++ b/mrs_lib/src/timer/timer.cpp.gcov.html @@ -0,0 +1,339 @@ + + + + + + + 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:10511492.1 %
Date:2024-04-09 22:03:33Functions:1515100.0 %
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             : /* running() //{ */
+      48             : 
+      49         215 : bool ROSTimer::running()
+      50             : {
+      51         430 :   std::scoped_lock lock(mutex_timer_);
+      52             : 
+      53         215 :   if (timer_)
+      54         215 :     return timer_->hasStarted();
+      55             :   else
+      56           0 :     return false;
+      57             : }
+      58             : 
+      59             : //}
+      60             : 
+      61             : // | ----------------------- ThreadTimer ---------------------- |
+      62             : 
+      63             : /* TheadTimer::start() //{ */
+      64             : 
+      65          24 : void ThreadTimer::start() {
+      66             : 
+      67          24 :   if (impl_) {
+      68          24 :     impl_->start();
+      69             :   }
+      70          24 : }
+      71             : 
+      72             : //}
+      73             : 
+      74             : /* ThreadTimer::stop() //{ */
+      75             : 
+      76          24 : void ThreadTimer::stop() {
+      77             : 
+      78          24 :   if (impl_) {
+      79          24 :     impl_->stop();
+      80             :   }
+      81          24 : }
+      82             : 
+      83             : //}
+      84             : 
+      85             : /* ThreadTimer::setPeriod() //{ */
+      86             : 
+      87           8 : void ThreadTimer::setPeriod(const ros::Duration& duration, [[maybe_unused]] const bool reset) {
+      88             : 
+      89           8 :   if (impl_) {
+      90           8 :     impl_->setPeriod(duration, reset);
+      91             :   }
+      92           8 : }
+      93             : 
+      94             : //}
+      95             : 
+      96             : /* ThreadTimer::running() //{ */
+      97             : 
+      98         208 : bool ThreadTimer::running()
+      99             : {
+     100         208 :   if (impl_)
+     101         208 :     return impl_->running_;
+     102             :   else
+     103           0 :     return false;
+     104             : }
+     105             : 
+     106             : //}
+     107             : 
+     108             : /* ThreadTimer::Impl //{ */
+     109             : 
+     110           8 : ThreadTimer::Impl::Impl(const std::function<void(const ros::TimerEvent&)>& callback, const ros::Duration& delay_dur, const bool oneshot)
+     111           8 :   : callback_(callback), oneshot_(oneshot), delay_dur_(delay_dur)
+     112             : {
+     113           8 :   ending_  = false;
+     114           8 :   running_ = false;
+     115             : 
+     116           8 :   last_real_     = ros::Time(0);
+     117           8 :   last_expected_ = ros::Time(0);
+     118           8 :   next_expected_ = ros::Time(0);
+     119             : 
+     120           8 :   thread_ = std::thread(&ThreadTimer::Impl::threadFcn, this);
+     121           8 : }
+     122             : 
+     123           8 : ThreadTimer::Impl::~Impl()
+     124             : {
+     125             :   {
+     126             :     // signal the thread to end
+     127          16 :     std::scoped_lock lck(mutex_wakeup_, mutex_state_);
+     128           8 :     ending_ = true;
+     129           8 :     wakeup_cond_.notify_all();
+     130             :   }
+     131             :   // wait for it to die
+     132           8 :   thread_.join();
+     133           8 : }
+     134             : 
+     135          24 : void ThreadTimer::Impl::start()
+     136             : {
+     137          48 :   std::scoped_lock lck(mutex_wakeup_, mutex_state_);
+     138          24 :   if (!running_)
+     139             :   {
+     140          24 :     next_expected_ = ros::Time::now() + delay_dur_;
+     141          24 :     running_ = true;
+     142          24 :     wakeup_cond_.notify_all();
+     143             :   }
+     144          24 : }
+     145             : 
+     146          24 : void ThreadTimer::Impl::stop()
+     147             : {
+     148          48 :   std::scoped_lock lck(mutex_wakeup_, mutex_state_);
+     149          24 :   running_ = false;
+     150          24 :   wakeup_cond_.notify_all();
+     151          24 : }
+     152             : 
+     153           8 : void ThreadTimer::Impl::setPeriod(const ros::Duration& duration, [[maybe_unused]] const bool reset)
+     154             : {
+     155          16 :   std::scoped_lock lock(mutex_wakeup_, mutex_state_);
+     156           8 :   delay_dur_ = duration;
+     157             :   // gracefully handle the special case
+     158           8 :   if (duration == ros::Duration(0))
+     159           0 :     this->oneshot_  = true;
+     160           8 : }
+     161             : 
+     162             : //}
+     163             : 
+     164             : /* ThreadTimer::breakableSleep() method //{ */
+     165         408 : bool ThreadTimer::Impl::breakableSleep(const ros::Time& until)
+     166             : {
+     167         408 :   while (ros::ok() && ros::Time::now() < until)
+     168             :   {
+     169         208 :     const std::chrono::nanoseconds dur {(until - ros::Time::now()).toNSec()};
+     170         208 :     const std::chrono::time_point<std::chrono::steady_clock> until_stl = std::chrono::steady_clock::now() + dur;
+     171         208 :     std::unique_lock lck(mutex_wakeup_);
+     172         208 :     if (!ending_ && running_)
+     173         208 :       wakeup_cond_.wait_until(lck, until_stl);
+     174             :     // check the flags while mutex_state_ is locked
+     175         208 :     if (ending_ || !running_)
+     176           8 :       return false;
+     177             :   }
+     178         200 :   return true;
+     179             : }
+     180             : //}
+     181             : 
+     182             : /* ThreadTimer::Impl::threadFcn() //{ */
+     183             : 
+     184         216 : void ThreadTimer::Impl::threadFcn()
+     185             : {
+     186         216 :   while (ros::ok() && !ending_)
+     187             :   {
+     188             :     {
+     189         216 :       std::unique_lock lck(mutex_wakeup_);
+     190             :       // if the timer is not yet started, wait for condition variable notification
+     191         216 :       if (!running_ && !ending_)
+     192          16 :         wakeup_cond_.wait(lck);
+     193             :       // The thread either got cv-notified, or running_ was already true, or it got woken up for some other reason.
+     194             :       // Check if the reason is that we should end (and end if it is).
+     195         216 :       if (ending_)
+     196           8 :         break;
+     197             :       // Check if the timer is still paused - probably was a spurious wake up (and restart the wait if it is).
+     198         208 :       if (!running_)
+     199           0 :         continue;
+     200             :     }
+     201             : 
+     202             :     // If the flow got here, the thread should attempt to wait for the specified duration.
+     203             :     // first, copy the delay we should wait for in a thread-safe manner
+     204         208 :     ros::Time next_expected;
+     205             :     {
+     206         208 :       std::scoped_lock lck(mutex_state_);
+     207         208 :       next_expected = next_expected_;
+     208             :     }
+     209         208 :     const bool interrupted = !breakableSleep(next_expected);
+     210         208 :     if (interrupted)
+     211           8 :       continue;
+     212             : 
+     213             :     {
+     214             :       // make sure the state doesn't change here
+     215         200 :       std::scoped_lock lck(mutex_state_);
+     216             :       // Again, check if everything is OK (the state may have changed while the thread was a sleeping beauty).
+     217             :       // Check if we should end (and end if it is so).
+     218         200 :       if (ending_)
+     219           0 :         break;
+     220             :       // Check if the timer is paused (and skip if it is so).
+     221         200 :       if (!running_)
+     222           0 :         continue;
+     223             : 
+     224             :       // If all is fine and dandy, actually run the callback function!
+     225             :       // if the timer is a oneshot-type, automatically pause it
+     226             :       // and do not fill out the expected fields in timer_event (we had no expectations...)
+     227         200 :       const ros::Time now = ros::Time::now();
+     228             :       // prepare the timer event
+     229         200 :       ros::TimerEvent timer_event;
+     230         200 :       if (oneshot_)
+     231             :       {
+     232           0 :         running_ = false;
+     233           0 :         timer_event.last_real        = last_real_;
+     234           0 :         timer_event.current_real     = now;
+     235             :       }
+     236             :       else
+     237             :       {
+     238         200 :         timer_event.last_expected    = last_expected_;
+     239         200 :         timer_event.last_real        = last_real_;
+     240         200 :         timer_event.current_expected = next_expected_;
+     241         200 :         timer_event.current_real     = now;
+     242             : 
+     243         200 :         last_expected_ = next_expected_;
+     244         200 :         next_expected_ = now + delay_dur_;
+     245             :       }
+     246         200 :       last_real_ = now;
+     247             :       // call the callback
+     248         200 :       callback_(timer_event);
+     249             :     }
+     250             :   }
+     251           8 : }
+     252             : 
+     253             : //}
+     254             : 
+     255             : }  // 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..4ce2f508b0 --- /dev/null +++ b/mrs_lib/src/timer/timer.cpp.gcov.overview.html @@ -0,0 +1,84 @@ + + + + + + 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 + + +
+ 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..e95d68d4697bd933416509c36f58cb8b9333f03f GIT binary patch literal 1067 zcmV+`1l0S9P)K3`}k|8a=Sbqh~@nKF8rC=ZW zMyHsWD!R5CSI=EeApq(V+p0jYM;YsFJC0*Nj_1~1j~*Zb<+i@Td-Co@t(Si0hwV%x zH^nJ)`?b=*sP{C>AW{6Y&BtvfY^nWD6Lx`LYQjmUi%Y8zngq(NCSAPKgo$GX_^l?K zb3Qzou$4#$3VYQU7hqn7n>qZCmf^_>_i>7t7_65j%s{wW=1z8vj5XHnSz`o6)AN)7 zM_7Xc8Pm@Bu_K)b9ND8i86?+#QpF8P>cp%C6tfN~uf^>OUu-uLV5ER+> z9+}+o@&U4dC{01UFqa^zQoO^~r+Qsw@XjfYH4IgSej=AETwlYGBFM5pKuIZ1bBfzd zID@s^gie^6WorBd_2kl=n$oxC6f_d!fWa9%6b0?)iG#WGCzqA4eT3ug*{v!dzP6RfXRr zz^Khk;sq$|M?phkI7&HA)0Nj2Y`}2S%&X2KR znDvA!dlquHgsW_&FjRDG$MTiF^m2{Y7Yxp@<3h?0?NFH2=XFafCd|q=4?;P+Ma=e lB+m7pa3ozqHtK%8@E@O{r@SB@n9u+K002ovPDHLkV1n4O`B4A> 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..2e1697ce91 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-09 22:03:33Functions: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..976005ff46 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-09 22:03:33Functions: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..7d53aa8899 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-09 22:03:33Functions: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..18fbba1134 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-09 22:03:33Functions: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..bcf7a5888b --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-09 22:03:33Functions: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..844ac3403e --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcasterHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-09 22:03:33Functions: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..dcbf9298a5 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcaster - transform_broadcaster.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-09 22:03:33Functions: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()155
mrs_lib::TransformBroadcaster::sendTransform(geometry_msgs::TransformStamped_<std::allocator<void> > const&)1725312
+
+
+ + + +
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..08624b6f69 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcaster - transform_broadcaster.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-09 22:03:33Functions: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&)1725312
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()155
+
+
+ + + +
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..b39f727e76 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.html @@ -0,0 +1,140 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transform_broadcaster - transform_broadcaster.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:163053.3 %
Date:2024-04-09 22:03:33Functions: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         155 : TransformBroadcaster::TransformBroadcaster() {
+       8         155 :   broadcaster_ = tf2_ros::TransformBroadcaster();
+       9         155 : }
+      10             : //}
+      11             : 
+      12             : /* sendTransform (const geometry_msgs::TransformStamped &transform) //{ */
+      13     1725312 : void TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped &transform) {
+      14     3449207 :   std::string frames_from_to = transform.header.frame_id + transform.child_frame_id;
+      15     1722679 :   if (last_messages_.count(frames_from_to) > 0) {
+      16     1723910 :     if (transform.header.stamp > last_messages_[frames_from_to]) {
+      17     1455586 :       broadcaster_.sendTransform(transform);
+      18     1457948 :       last_messages_[frames_from_to] = transform.header.stamp;
+      19             :     } else {
+      20      265250 :       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        1610 :     std::pair<std::string, ros::Time> new_pair;
+      25         794 :     new_pair.first  = frames_from_to;
+      26         794 :     new_pair.second = transform.header.stamp;
+      27         794 :     broadcaster_.sendTransform(transform);
+      28         794 :     last_messages_.insert(new_pair);
+      29             :   }
+      30     1725581 : }
+      31             : //}
+      32             : 
+      33             : /* sendTransform(const std::vector<geometry_msgs::TransformStamped> &transforms) //{ */
+      34           0 : void TransformBroadcaster::sendTransform(const std::vector<geometry_msgs::TransformStamped> &transforms) {
+      35           0 :   for (auto transform : transforms) {
+      36           0 :     std::string frames_from_to = transform.header.frame_id + transform.child_frame_id;
+      37           0 :     if (last_messages_.count(frames_from_to) > 0) {
+      38           0 :       if (transform.header.stamp > last_messages_[frames_from_to]) {
+      39           0 :         broadcaster_.sendTransform(transform);
+      40           0 :         last_messages_[frames_from_to] = transform.header.stamp;
+      41             :       } else {
+      42           0 :         ROS_WARN_ONCE("[%s]: TF_REPEATED_DATA ignoring data with redundant timestamp. Transform from frame '%s' to frame '%s'",
+      43             :                       ros::this_node::getName().c_str(), transform.header.frame_id.c_str(), transform.child_frame_id.c_str());
+      44             :       }
+      45             :     } else {
+      46           0 :       std::pair<std::string, ros::Time> new_pair;
+      47           0 :       new_pair.first  = frames_from_to;
+      48           0 :       new_pair.second = transform.header.stamp;
+      49           0 :       broadcaster_.sendTransform(transform);
+      50           0 :       last_messages_.insert(new_pair);
+      51             :     }
+      52             :   }
+      53           0 : }
+      54             : //}
+      55             : 
+      56             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.overview.html b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.overview.html new file mode 100644 index 0000000000..f1fb284078 --- /dev/null +++ b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.overview.html @@ -0,0 +1,34 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.png b/mrs_lib/src/transform_broadcaster/transform_broadcaster.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..ef243e6e5fb1d3bcaec6f7b6354ea8ce8ebffdea GIT binary patch literal 425 zcmV;a0apHrP)M01uJ*nY`JnFx*N2@5{2?`a1q6zmssfL3_>!}8X35VVlYkpKa402t<;> + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-09 22:03:33Functions: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..a58495a78a --- /dev/null +++ b/mrs_lib/src/transformer/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-09 22:03:33Functions: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..3e6a33aa5b --- /dev/null +++ b/mrs_lib/src/transformer/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-09 22:03:33Functions: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..12d02933b5 --- /dev/null +++ b/mrs_lib/src/transformer/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-09 22:03:33Functions: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..cd005a2044 --- /dev/null +++ b/mrs_lib/src/transformer/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-09 22:03:33Functions: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..4fff8c0d63 --- /dev/null +++ b/mrs_lib/src/transformer/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-09 22:03:33Functions: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..d9428daade --- /dev/null +++ b/mrs_lib/src/transformer/transformer.cpp.func-sort-c.html @@ -0,0 +1,184 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer/transformer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformer - transformer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-09 22:03:33Functions: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&)569
mrs_lib::Transformer::UTMtoLL(geometry_msgs::PoseStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1791
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Pose_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1791
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1793
mrs_lib::Transformer::getFramePrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3592
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&)22468
mrs_lib::Transformer::transformImpl(geometry_msgs::TransformStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)102114
mrs_lib::Transformer::setLatLon(double, double)172046
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&)1430067
mrs_lib::Transformer::resolveFrameImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)6012565
+
+
+ + + +
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..88883617a8 --- /dev/null +++ b/mrs_lib/src/transformer/transformer.cpp.func.html @@ -0,0 +1,184 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer/transformer.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformer - transformer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-09 22:03:33Functions: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&)22468
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&)102114
mrs_lib::Transformer::getFramePrefix(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)3592
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&)1430067
mrs_lib::Transformer::resolveFrameImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)6012565
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&)1791
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&)1791
mrs_lib::Transformer::UTMtoLL(geometry_msgs::Point_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1793
mrs_lib::Transformer::setLatLon(double, double)172046
mrs_lib::Transformer::Transformer(ros::NodeHandle const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Duration const&)569
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..5559f7d133 --- /dev/null +++ b/mrs_lib/src/transformer/transformer.cpp.gcov.html @@ -0,0 +1,671 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer/transformer.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_lib/src/transformer - transformer.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:18528066.1 %
Date:2024-04-09 22:03:33Functions: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         569 :   Transformer::Transformer(const ros::NodeHandle& nh, const std::string& node_name, const ros::Duration& cache_time)
+      49         569 :     : 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         569 :   }
+      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       22468 :   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       44964 :     std::scoped_lock lck(mutex_);
+      81             : 
+      82       22496 :     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       44992 :     const std::string from_frame = resolveFrameImpl(from_frame_raw);
+      90       44992 :     const std::string to_frame = resolveFrameImpl(to_frame_raw);
+      91       44992 :     const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
+      92             : 
+      93       22496 :     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      172046 :   void Transformer::setLatLon(const double lat, const double lon)
+     118             :   {
+     119      172046 :     std::scoped_lock lck(mutex_);
+     120             : 
+     121             :     double utm_x, utm_y;
+     122      171403 :     mrs_lib::LLtoUTM(lat, lon, utm_y, utm_x, utm_zone_.data());
+     123      171278 :     got_utm_zone_ = true;
+     124      170724 :   }
+     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      102114 :   std::optional<mrs_msgs::ReferenceStamped> Transformer::transformImpl(const geometry_msgs::TransformStamped& tf, const mrs_msgs::ReferenceStamped& what)
+     253             :   {
+     254             :     // create a pose message
+     255      204228 :     geometry_msgs::PoseStamped pose;
+     256      102114 :     pose.header = what.header;
+     257             :   
+     258      102114 :     pose.pose.position.x = what.reference.position.x;
+     259      102114 :     pose.pose.position.y = what.reference.position.y;
+     260      102114 :     pose.pose.position.z = what.reference.position.z;
+     261      102114 :     pose.pose.orientation = geometry::fromEigen(geometry::quaternionFromHeading(what.reference.heading));
+     262             :   
+     263             :     // try to transform the pose message
+     264      204228 :     const auto pose_opt = transformImpl(tf, pose);
+     265      102114 :     if (!pose_opt.has_value())
+     266           0 :       return std::nullopt;
+     267             :     // overwrite the pose with it's transformed value
+     268      102114 :     pose = pose_opt.value();
+     269             :   
+     270      204228 :     mrs_msgs::ReferenceStamped ret;
+     271      102114 :     ret.header = pose.header;
+     272             :   
+     273             :     // copy the new transformed data back
+     274      102114 :     ret.reference.position.x = pose.pose.position.x;
+     275      102114 :     ret.reference.position.y = pose.pose.position.y;
+     276      102114 :     ret.reference.position.z = pose.pose.position.z;
+     277      102114 :     ret.reference.heading = geometry::headingFromRot(geometry::toEigen(pose.pose.orientation));
+     278      102114 :     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     1430067 :   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     1430067 :     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     1430067 :     if (from_frame.empty() || to_frame.empty())
+     312             :     {
+     313        1060 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Transformer: cannot transform to/from an empty frame!", node_name_.c_str());
+     314        1060 :       return std::nullopt;
+     315             :     }
+     316             : 
+     317             :     // if the frames are the same, just return an identity transform
+     318     1428997 :     if (from_frame == to_frame)
+     319      328498 :       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     1264758 :     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     1264759 :     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        3588 :       const std::string utm_frame = getFramePrefix(to_frame) + "utm_origin";
+     337        3588 :       auto tf_opt = getTransformImpl(from_frame, utm_frame, time_stamp, latlon_frame);
+     338        1794 :       if (!tf_opt.has_value())
+     339           0 :         return std::nullopt;
+     340             :       // change the transformation frames to point to latlon
+     341        1794 :       frame_to(*tf_opt) = to_frame;
+     342        1794 :       return tf_opt;
+     343             :     }
+     344             : 
+     345     3788839 :     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     1741478 :       return tf_buffer_->lookupTransform(to_frame, from_frame, time_stamp, lookup_timeout_);
+     351             :     }
+     352     1568847 :     catch (tf2::TransformException& e)
+     353             :     {
+     354      784420 :       ex = e;
+     355             :     }
+     356             : 
+     357             :     // if that failed, try to get the newest one if requested
+     358      784416 :     if (retry_lookup_newest_)
+     359             :     {
+     360             :       try
+     361             :       {
+     362     1556143 :         return tf_buffer_->lookupTransform(to_frame, from_frame, ros::Time(0), lookup_timeout_);
+     363             :       }
+     364       25206 :       catch (tf2::TransformException& e)
+     365             :       {
+     366       12603 :         ex = e;
+     367             :       }
+     368             :     }
+     369             : 
+     370             :     // if the flow got here, we've failed to look the transform up
+     371       12635 :     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       12635 :       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       12635 :     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     6012565 :   std::string Transformer::resolveFrameImpl(const std::string& frame_id)
+     464             :   {
+     465             :     // if the frame is empty, return the default frame id
+     466     6012565 :     if (frame_id.empty())
+     467        6457 :       return default_frame_id_;
+     468             : 
+     469             :     // if there is no prefix set, just return the raw frame id
+     470     6006109 :     if (prefix_.empty())
+     471     3740633 :       return frame_id;
+     472             : 
+     473             :     // if there is a default prefix set and the frame does not start with it, prefix it
+     474     2265393 :     if (frame_id.substr(0, prefix_.length()) != prefix_)
+     475     1424323 :       return prefix_ + frame_id;
+     476             : 
+     477      841182 :     return frame_id;
+     478             :   }
+     479             : 
+     480             :   //}
+     481             : 
+     482             :   /* LLtoUTM() method //{ */
+     483           2 :   geometry_msgs::Point Transformer::LLtoUTM(const geometry_msgs::Point& what, [[maybe_unused]] const std::string& prefix)
+     484             :   {
+     485             :     // convert LAT-LON to UTM
+     486           2 :     geometry_msgs::Point utm;
+     487           2 :     mrs_lib::UTM(what.x, what.y, &utm.x, &utm.y);
+     488             :     // copy the height from the input
+     489           2 :     utm.z = what.z;
+     490           2 :     return utm;
+     491             :   }
+     492             : 
+     493           0 :   geometry_msgs::PointStamped Transformer::LLtoUTM(const geometry_msgs::PointStamped& what, [[maybe_unused]] const std::string& prefix)
+     494             :   {
+     495           0 :     geometry_msgs::PointStamped ret;
+     496           0 :     ret.header.frame_id = prefix + "utm_origin";
+     497           0 :     ret.header.stamp = what.header.stamp;
+     498           0 :     ret.point = LLtoUTM(what.point, prefix);
+     499           0 :     return ret;
+     500             :   }
+     501             : 
+     502           0 :   geometry_msgs::Pose Transformer::LLtoUTM(const geometry_msgs::Pose& what, const std::string& prefix)
+     503             :   {
+     504           0 :     geometry_msgs::Pose ret;
+     505           0 :     ret.position = LLtoUTM(what.position, prefix);
+     506           0 :     ret.orientation = what.orientation;
+     507           0 :     return ret;
+     508             :   }
+     509             : 
+     510           0 :   geometry_msgs::PoseStamped Transformer::LLtoUTM(const geometry_msgs::PoseStamped& what, const std::string& prefix)
+     511             :   {
+     512           0 :     geometry_msgs::PoseStamped ret;
+     513           0 :     ret.header.frame_id = prefix + "utm_origin";
+     514           0 :     ret.header.stamp = what.header.stamp;
+     515           0 :     ret.pose = LLtoUTM(what.pose, prefix);
+     516           0 :     return ret;
+     517             :   }
+     518             :   //}
+     519             : 
+     520             :   /* UTMtoLL() method //{ */
+     521        1793 :   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        1793 :     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        1792 :     geometry_msgs::Point latlon;
+     532        1792 :     mrs_lib::UTMtoLL(what.y, what.x, utm_zone_.data(), latlon.x, latlon.y);
+     533        1792 :     latlon.z = what.z;
+     534        1792 :     return latlon;
+     535             :   }
+     536             : 
+     537           0 :   std::optional<geometry_msgs::PointStamped> Transformer::UTMtoLL(const geometry_msgs::PointStamped& what, [[maybe_unused]] const std::string& prefix)
+     538             :   {
+     539           0 :     const auto opt = UTMtoLL(what.point, prefix);
+     540           0 :     if (!opt.has_value())
+     541           0 :       return std::nullopt;
+     542             : 
+     543           0 :     geometry_msgs::PointStamped ret;
+     544           0 :     ret.header.frame_id = prefix + "utm_origin";
+     545           0 :     ret.header.stamp = what.header.stamp;
+     546           0 :     ret.point = opt.value();
+     547           0 :     return ret;
+     548             :   }
+     549             : 
+     550        1791 :   std::optional<geometry_msgs::Pose> Transformer::UTMtoLL(const geometry_msgs::Pose& what, const std::string& prefix)
+     551             :   {
+     552        1791 :     const auto opt = UTMtoLL(what.position, prefix);
+     553        1791 :     if (!opt.has_value())
+     554           0 :       return std::nullopt;
+     555             : 
+     556        1791 :     geometry_msgs::Pose ret;
+     557        1791 :     ret.position = opt.value();
+     558        1791 :     ret.orientation = what.orientation;
+     559        1791 :     return ret;
+     560             :   }
+     561             : 
+     562        1791 :   std::optional<geometry_msgs::PoseStamped> Transformer::UTMtoLL(const geometry_msgs::PoseStamped& what, const std::string& prefix)
+     563             :   {
+     564        1791 :     const auto opt = UTMtoLL(what.pose, prefix);
+     565        1791 :     if (!opt.has_value())
+     566           0 :       return std::nullopt;
+     567             : 
+     568        3582 :     geometry_msgs::PoseStamped ret;
+     569        1791 :     ret.header.frame_id = prefix + "utm_origin";
+     570        1791 :     ret.header.stamp = what.header.stamp;
+     571        1791 :     ret.pose = opt.value();
+     572        1791 :     return ret;
+     573             :   }
+     574             :   //}
+     575             : 
+     576             :   /* getFramePrefix() method //{ */
+     577        3592 :   std::string Transformer::getFramePrefix(const std::string& frame_id)
+     578             :   {
+     579        3592 :     const auto firstof = frame_id.find_first_of('/');
+     580        3592 :     if (firstof == std::string::npos)
+     581           0 :       return "";
+     582             :     else
+     583        3592 :       return frame_id.substr(0, firstof+1);
+     584             :   }
+     585             :   //}
+     586             : 
+     587             : }  // namespace mrs_lib
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_lib/src/transformer/transformer.cpp.gcov.overview.html b/mrs_lib/src/transformer/transformer.cpp.gcov.overview.html new file mode 100644 index 0000000000..18b7b2cf49 --- /dev/null +++ b/mrs_lib/src/transformer/transformer.cpp.gcov.overview.html @@ -0,0 +1,167 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_lib/src/transformer/transformer.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_autostart::automatic_start::AutomaticStart::disarm()3
mrs_uav_autostart::automatic_start::AutomaticStart::takeoff()18
mrs_uav_autostart::automatic_start::AutomaticStart::toggleControlOutput(bool const&)24
(anonymous namespace)::ProxyExec0::ProxyExec0()25
mrs_uav_autostart::automatic_start::AutomaticStart::onInit()25
mrs_uav_autostart::automatic_start::AutomaticStart::changeState(mrs_uav_autostart::automatic_start::LandingStates_t)41
mrs_uav_autostart::automatic_start::Topic::Topic(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)51
mrs_uav_autostart::automatic_start::Topic::getTopicName[abi:cxx11]()73
mrs_uav_autostart::automatic_start::AutomaticStart::callbackGazeboSpawnerDiagnostics(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)3112
mrs_uav_autostart::automatic_start::AutomaticStart::topicCheck()9516
mrs_uav_autostart::automatic_start::AutomaticStart::validateReference()9516
mrs_uav_autostart::automatic_start::AutomaticStart::isGazeboSimulation()9516
mrs_uav_autostart::automatic_start::AutomaticStart::preflighCheckGyro()9517
mrs_uav_autostart::automatic_start::AutomaticStart::preflighCheckHeight()9517
mrs_uav_autostart::automatic_start::AutomaticStart::preflightCheckSpeed()9517
mrs_uav_autostart::automatic_start::AutomaticStart::timerMain(ros::TimerEvent const&)17005
mrs_uav_autostart::automatic_start::Topic::getTime()38137
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)46463
mrs_uav_autostart::automatic_start::Topic::updateTime()46468
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&) const46475
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)109841
+
+
+ + + +
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..923e1c3bfd --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.func.html @@ -0,0 +1,164 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src/automatic_start.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/src - automatic_start.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:28432587.4 %
Date:2024-04-09 22:03:33Functions:2121100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()25
mrs_uav_autostart::automatic_start::AutomaticStart::topicCheck()9516
mrs_uav_autostart::automatic_start::AutomaticStart::changeState(mrs_uav_autostart::automatic_start::LandingStates_t)41
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)46463
mrs_uav_autostart::automatic_start::AutomaticStart::preflighCheckGyro()9517
mrs_uav_autostart::automatic_start::AutomaticStart::validateReference()9516
mrs_uav_autostart::automatic_start::AutomaticStart::isGazeboSimulation()9516
mrs_uav_autostart::automatic_start::AutomaticStart::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)109841
mrs_uav_autostart::automatic_start::AutomaticStart::preflighCheckHeight()9517
mrs_uav_autostart::automatic_start::AutomaticStart::preflightCheckSpeed()9517
mrs_uav_autostart::automatic_start::AutomaticStart::toggleControlOutput(bool const&)24
mrs_uav_autostart::automatic_start::AutomaticStart::callbackGazeboSpawnerDiagnostics(boost::shared_ptr<mrs_msgs::GazeboSpawnerDiagnostics_<std::allocator<void> > const>)3112
mrs_uav_autostart::automatic_start::AutomaticStart::disarm()3
mrs_uav_autostart::automatic_start::AutomaticStart::onInit()25
mrs_uav_autostart::automatic_start::AutomaticStart::takeoff()18
mrs_uav_autostart::automatic_start::AutomaticStart::timerMain(ros::TimerEvent const&)17005
mrs_uav_autostart::automatic_start::Topic::updateTime()46468
mrs_uav_autostart::automatic_start::Topic::getTopicName[abi:cxx11]()73
mrs_uav_autostart::automatic_start::Topic::getTime()38137
mrs_uav_autostart::automatic_start::Topic::Topic(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)51
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&) const46475
+
+
+ + + +
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..33b5d55258 --- /dev/null +++ b/mrs_uav_autostart/src/automatic_start.cpp.gcov.html @@ -0,0 +1,1049 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_autostart/src/automatic_start.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_autostart/src - automatic_start.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:28432587.4 %
Date:2024-04-09 22:03:33Functions:2121100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_autostart/src/automatic_start.cpp.gcov.png b/mrs_uav_autostart/src/automatic_start.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..274326d00aecff22c394aa9d9573cc9f11f50a25 GIT binary patch literal 3123 zcmV-349xS1P)Q-vhZ2T8cgZN1%Y+xu&g z8gG0C>8*t_0y2O*r?z8b0rp@d!P$W9!KQHxBQ2Z`z`?02s8K+S0t65zl$g2=$lE>L zQ%{LA!^j1knUa`V0IkT*N->`DCygmZ>Hr*V^^k>dgk#cP;3a+Cj9vQRQf0KoOSc>_4d zbtJZ2n()4~;iM4{YYKjTg5?Er?YwKs_YBv5V3rpuSE2H`5Q-m90XHypCbsGH2G|s< z`APzuQB~j^G_4$OF6`-^rZIILqn}zJm)~pY8XbO5~`SNTxU!Kg1G2cs(=eLrxm~j09M#eG1imzOKQny z1&PTKIM;Cl;18}f@uZH0RZ!wHv%0@#4?e6azz~gFl=+-5T#`_R^bEH=7^gAj5tbm= zeqCg_rq!$9nqs@;db;>ga6QR%&NXdQa;_^FbFTZT$}k45oiPy&hO3SQQV;DKf)SvB zZ-zH|D!xj@lI}1GPaE?)F1p^_Cu<>|+&(5_bN2FUve&Xr4YBG{FKYA@Ns{^!Kt7JK zQH~nI`2JV7>oZBgj4p)q){yr7tY6jZT)u##>U47j<$F(DrW?a*95VtmGsmHx|t>c0i_;(l14;nh^fKW)gt0S zr=G%C5PBRU7!#~5zyK*Wb&ymO(9bP4by%Yg@%*&F*?@onCgPGmset#w7aehBu z&-a%dcRK*!y*ifrvknjd`9r*NRtWr*7P;0zZo1Ox;xTzZC-eI5(U@P&0ky+OxV&=e zCZNMeyWsLD0z`20(HBHY9aRv$Y zY?MZc&VoeXZW@eS%rv8XrQ58Ek-dba;Lx2hCP$ESU1#J7NunCvx-3P?Ho~!ys_w$5 zPuVGruAK6Yu}irkCB>?YaSbG?G$~4h(a&lp*4pn#odL4WNF8f^&a6>JOJH@-U2jXyF|d2Ekpc2fx!3f4=cURd zSvzWRDW4EOp707===T%wT#W{3CXH~VRn9B~WacwEroJfkSs7adOn>f{<(L5=6XO7l zQdbsGALQ2&Y-eO;jD=FUEk|PtbP2y~QwT|%5P1Hf8afTvFE*G%F5~=q!66qtk}6#o zivin{N)7?!V)TvnTT3%{Pkk*W?3;Qy+xkLbeT}Ubw%IRE?H9Yc(*2>rUOQStK}&$Q1*o{xmmx z`|Lz;mWK_)c{5TUhA~DQ3i=MFS54nPu@(BX~Pq-#5xxfJIN!_Iecty`g zWLuZ{dcaK9qg6->1KbOYy1d!(y0h$NIXZ|-Wx!Uz2`II zq}>$c!jg8^p<+o5=@z@<1q^9vB*7S|K!$U63-GAs*@pd$PfrPnr_xUajeQLDFn)&I zhlVG!Zo(5Cad-e3dJrF3dqLDN+Nd~soW{dam{fG-YYbng7XUR z7mt>a+8sAwwJS7o;_!3G9wAK3;<2xkh&t|!H?_e>fb+Jj|kX^ESz2o2OsBS!6qKB zl@A`%-bgWGu7%08;rfzl+-CFTq3DA^?a*_j{!5j+hrMyV4RM=aWkyirfS@!MY~--c z6v1QUZdhkp1u<0T5kR`_v5JwZ7w&u)#~!lE<#|!a3d5Iko=dq#47i&?$7{`kg~ zJNQsyOpJ5}SigTQEgr7r`OAgH!mj*4_pDrHk?Jf^nc?fJrr_Jwc)3&e&AXBMj8rw{ zjD&5ZTtBlf)&I=^n^!L#Ah&IYJLH|_o~8CF!TUUJ98Y`ZI4`K40)DhU8PYNF{Er_c#=RDEx%knB zTz?~ebiwfd__2!dD;}iV?m+ItNHthM%_m*_nNHW`Q{;Psm&|@H%Y3*Sq|S#!np42a!y0<* zBfp}ukYF+x2QQ7*Ra;bN>jxKsg|jy7JDnr+?%j8I+k z^>izWXwliWych-E&VaYN=m7V9Fn_LUB~zx)@;4S zYlR!UV7Lz4o?Cu0kBe*yrodCJt{hW*vY;7gUY0EInhnP=*Eac0wN)o3g)hg{+@Xq_ z|JsB{$mA4qB=SA}*zt79pGyRu3I?(BLEv%gq#{h-_N1EHw-pVbd>sD2G*sKXqb8+t zR`{SvNq*7c7JlX5#BVyppG0Cm5s@RkGQUKQ=gqin^3gA)?IAmSKYS1`%)K2sjm)Xt zQ&pO+61n>bO}R+BGVMMdrdD8cSU=!CvTOWFr%PluTXgs%|J{ca#vgj*cB!%#oQ8ji z+4~yzLH%NoUm{0&_fhS$|M+^p|;(xfva8J`)KHmTU N002ovPDHLkV1f}s3SIyJ literal 0 HcmV?d00001 diff --git a/mrs_uav_autostart/src/index-detail-sort-f.html b/mrs_uav_autostart/src/index-detail-sort-f.html new file mode 100644 index 0000000000..109a295d0a --- /dev/null +++ b/mrs_uav_autostart/src/index-detail-sort-f.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:28432587.4 %
Date:2024-04-09 22:03:33Functions:2121100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
87.4%87.4%
+ 		87.4 %284 / 325100.0 %21 / 21
<unnamed>87.4 %284 / 325100.0 %21 / 21
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index-detail-sort-l.html b/mrs_uav_autostart/src/index-detail-sort-l.html new file mode 100644 index 0000000000..c75636beb0 --- /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:28432587.4 %
Date:2024-04-09 22:03:33Functions:2121100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
87.4%87.4%
+ 		87.4 %284 / 325100.0 %21 / 21
<unnamed>87.4 %284 / 325100.0 %21 / 21
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index-detail.html b/mrs_uav_autostart/src/index-detail.html new file mode 100644 index 0000000000..3ca0829e62 --- /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:28432587.4 %
Date:2024-04-09 22:03:33Functions:2121100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
87.4%87.4%
+ 		87.4 %284 / 325100.0 %21 / 21
<unnamed>87.4 %284 / 325100.0 %21 / 21
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index-sort-f.html b/mrs_uav_autostart/src/index-sort-f.html new file mode 100644 index 0000000000..63c6ebe82e --- /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:28432587.4 %
Date:2024-04-09 22:03:33Functions:2121100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
87.4%87.4%
+ 		87.4 %284 / 325100.0 %21 / 21
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index-sort-l.html b/mrs_uav_autostart/src/index-sort-l.html new file mode 100644 index 0000000000..9dafcd40be --- /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:28432587.4 %
Date:2024-04-09 22:03:33Functions:2121100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
87.4%87.4%
+ 		87.4 %284 / 325100.0 %21 / 21
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_autostart/src/index.html b/mrs_uav_autostart/src/index.html new file mode 100644 index 0000000000..27a43bf260 --- /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:28432587.4 %
Date:2024-04-09 22:03:33Functions:2121100.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
automatic_start.cpp +
87.4%87.4%
+ 		87.4 %284 / 325100.0 %21 / 21
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/common.h.func-sort-c.html b/mrs_uav_controllers/include/common.h.func-sort-c.html new file mode 100644 index 0000000000..73b115caea --- /dev/null +++ b/mrs_uav_controllers/include/common.h.func-sort-c.html @@ -0,0 +1,116 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/common.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - common.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:242596.0 %
Date:2024-04-09 22:03:33Functions: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&)9
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)26
mrs_uav_controllers::common::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)36
+
+
+ + + +
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..88dce25aff --- /dev/null +++ b/mrs_uav_controllers/include/common.h.func.html @@ -0,0 +1,116 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/common.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - common.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:242596.0 %
Date:2024-04-09 22:03:33Functions: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&)26
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&)36
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&)9
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..2cceb14b32 --- /dev/null +++ b/mrs_uav_controllers/include/common.h.gcov.html @@ -0,0 +1,189 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/common.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - common.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:242596.0 %
Date:2024-04-09 22:03:33Functions: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          26 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeCmd& msg) {
+      77          26 :     return msg.throttle;
+      78             :   }
+      79          36 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+      80          36 :     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           9 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+      89           9 :     return std::nullopt;
+      90             :   }
+      91           4 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+      92           4 :     return std::nullopt;
+      93             :   }
+      94           2 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiPositionCmd& msg) {
+      95           2 :     return std::nullopt;
+      96             :   }
+      97             : };
+      98             : 
+      99             : //}
+     100             : 
+     101             : }  // namespace common
+     102             : 
+     103             : }  // namespace mrs_uav_controllers
+     104             : 
+     105             : #endif  // MRS_UAV_CONTROLLERS_COMMON_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/include/common.h.gcov.overview.html b/mrs_uav_controllers/include/common.h.gcov.overview.html new file mode 100644 index 0000000000..42538a882f --- /dev/null +++ b/mrs_uav_controllers/include/common.h.gcov.overview.html @@ -0,0 +1,47 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/common.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_controllers/include/common.h.gcov.png b/mrs_uav_controllers/include/common.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..042ffaa6e8f2e8a56ad432456b5e16b86c9a1a42 GIT binary patch literal 436 zcmV;l0ZaagP)#^9ia;?Oh<);&1n;{`V)#)grKCzA%7BS577Yq& z5+&x4JP}o7{Ly42B6iV-v91#DuqLc}xHa5d_Yk2#;Y9#97rdz8W<^)%tNpgB`AL_}T^7{lT~x##yVi&>&{wCSk3s5OGCw1ZkFV z%0%z|G)=E*g+Iqkd^4{jB8)X_XUth_U0K_;igU^)%HNq^#npTNew-p^-I{gioHPB7 z?x}<-bgj~XSncp{k4M(L`AF-qxc+bb*R1zZJn>jR))%aAxLyzcjgutzGk?4ydjY_) e#FpJEikcq_FD>HrpF4K|0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/includeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:575898.3 %
Date:2024-04-09 22:03:33Functions: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..be21c67621 --- /dev/null +++ b/mrs_uav_controllers/include/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/includeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:575898.3 %
Date:2024-04-09 22:03:33Functions: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..f0f62e024b --- /dev/null +++ b/mrs_uav_controllers/include/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/includeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:575898.3 %
Date:2024-04-09 22:03:33Functions: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..b92992bb74 --- /dev/null +++ b/mrs_uav_controllers/include/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/includeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:575898.3 %
Date:2024-04-09 22:03:33Functions: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..2c42482243 --- /dev/null +++ b/mrs_uav_controllers/include/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/includeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:575898.3 %
Date:2024-04-09 22:03:33Functions: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..0c12c6df2c --- /dev/null +++ b/mrs_uav_controllers/include/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/includeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:575898.3 %
Date:2024-04-09 22:03:33Functions: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..740d97070e --- /dev/null +++ b/mrs_uav_controllers/include/pid.hpp.func-sort-c.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/pid.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - pid.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3333100.0 %
Date:2024-04-09 22:03:33Functions:55100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::PIDController::reset()984
mrs_uav_controllers::PIDController::setParams(double const&, double const&, double const&, double const&, double const&)984
mrs_uav_controllers::PIDController::PIDController()984
mrs_uav_controllers::PIDController::setSaturation(double)13501
mrs_uav_controllers::PIDController::update(double const&, double const&)13501
+
+
+ + + +
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..b8f4d71fff --- /dev/null +++ b/mrs_uav_controllers/include/pid.hpp.func.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/pid.hpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - pid.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3333100.0 %
Date:2024-04-09 22:03:33Functions:55100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::PIDController::setSaturation(double)13501
mrs_uav_controllers::PIDController::reset()984
mrs_uav_controllers::PIDController::update(double const&, double const&)13501
mrs_uav_controllers::PIDController::setParams(double const&, double const&, double const&, double const&, double const&)984
mrs_uav_controllers::PIDController::PIDController()984
+
+
+ + + +
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..b02d9b3722 --- /dev/null +++ b/mrs_uav_controllers/include/pid.hpp.gcov.html @@ -0,0 +1,184 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/include/pid.hpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/include - pid.hpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:3333100.0 %
Date:2024-04-09 22:03:33Functions: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         984 : PIDController::PIDController() {
+      43             : 
+      44         984 :   this->reset();
+      45         984 : }
+      46             : 
+      47         984 : void PIDController::setParams(const double &kp, const double &kd, const double &ki, const double &saturation, const double &antiwindup) {
+      48             : 
+      49         984 :   this->_kp_       = kp;
+      50         984 :   this->_kd_       = kd;
+      51         984 :   this->_ki_       = ki;
+      52         984 :   this->saturation = saturation;
+      53         984 :   this->antiwindup = antiwindup;
+      54         984 : }
+      55             : 
+      56       13501 : void PIDController::setSaturation(const double saturation) {
+      57             : 
+      58       13501 :   this->saturation = saturation;
+      59       13501 : }
+      60             : 
+      61         984 : void PIDController::reset(void) {
+      62             : 
+      63         984 :   this->last_error_ = 0;
+      64         984 :   this->integral_   = 0;
+      65         984 : }
+      66             : 
+      67       13501 : double PIDController::update(const double &error, const double &dt) {
+      68             : 
+      69             :   // calculate the control error difference
+      70       13501 :   double difference = (error - last_error_) / dt;
+      71       13501 :   last_error_       = error;
+      72             : 
+      73       13501 :   double p_component = _kp_ * error;       // proportional feedback
+      74       13501 :   double d_component = _kd_ * difference;  // derivative feedback
+      75       13501 :   double i_component = _ki_ * integral_;   // derivative feedback
+      76             : 
+      77       13501 :   double sum = p_component + d_component + i_component;
+      78             : 
+      79       13501 :   if (saturation > 0) {
+      80       13501 :     if (sum >= saturation) {
+      81         132 :       sum = saturation;
+      82       13369 :     } else if (sum <= -saturation) {
+      83         153 :       sum = -saturation;
+      84             :     }
+      85             :   }
+      86             : 
+      87       13501 :   if (antiwindup > 0) {
+      88       13501 :     if (std::abs(sum) < antiwindup) {
+      89             :       // add to the integral
+      90       12882 :       integral_ += error * dt;
+      91             :     }
+      92             :   }
+      93             : 
+      94             :   // return the summ of the components
+      95       13501 :   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..93ae1aa79c --- /dev/null +++ b/mrs_uav_controllers/src/common.cpp.func-sort-c.html @@ -0,0 +1,116 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/common.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - common.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415984.3 %
Date:2024-04-09 22:03:33Functions: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&)199
mrs_uav_controllers::common::actuatorMixer(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)3798
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&)7625
mrs_uav_controllers::common::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)9952
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&)70728
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&)70728
mrs_uav_controllers::common::orientationError(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)72697
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&)72697
mrs_uav_controllers::common::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)76652
+
+
+ + + +
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..b237af2f78 --- /dev/null +++ b/mrs_uav_controllers/src/common.cpp.func.html @@ -0,0 +1,116 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/common.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - common.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415984.3 %
Date:2024-04-09 22:03:33Functions: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&)70728
mrs_uav_controllers::common::actuatorMixer(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&)3798
mrs_uav_controllers::common::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)76652
mrs_uav_controllers::common::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)199
mrs_uav_controllers::common::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)9952
mrs_uav_controllers::common::orientationError(Eigen::Matrix<double, 3, 3, 0, 3, 3> const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)72697
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&)72697
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&)70728
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&)7625
+
+
+ + + +
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..ff40e5663e --- /dev/null +++ b/mrs_uav_controllers/src/common.cpp.gcov.html @@ -0,0 +1,471 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/common.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - common.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415984.3 %
Date:2024-04-09 22:03:33Functions: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       72697 : Eigen::Vector3d orientationError(const Eigen::Matrix3d& R, const Eigen::Matrix3d& Rd) {
+      12             : 
+      13             :   // orientation error
+      14       72697 :   Eigen::Matrix3d R_error = 0.5 * (Rd.transpose() * R - R.transpose() * Rd);
+      15             : 
+      16             :   // vectorize the orientation error
+      17             :   // clang-format off
+      18       72697 :     Eigen::Vector3d R_error_vec;
+      19       72697 :     R_error_vec << (R_error(1, 2) - R_error(2, 1)) / 2.0,
+      20       72697 :                    (R_error(2, 0) - R_error(0, 2)) / 2.0,
+      21       72697 :                    (R_error(0, 1) - R_error(1, 0)) / 2.0;
+      22             :   // clang-format on
+      23             : 
+      24      145394 :   return R_error_vec;
+      25             : }
+      26             : 
+      27             : //}
+      28             : 
+      29             : /* sanitizeDesiredForce() //{ */
+      30             : 
+      31       70728 : 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       70728 :   double theta = acos(input[2]);
+      36       70728 :   double phi   = atan2(input[1], input[0]);
+      37             : 
+      38             :   // check for the failsafe limit
+      39       70728 :   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       70728 :   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       70728 :   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       70728 :   Eigen::Vector3d output(sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta));
+      63             : 
+      64       70728 :   return {output};
+      65             : }
+      66             : 
+      67             : //}
+      68             : 
+      69             : /* so3transform() //{ */
+      70             : 
+      71       70728 : Eigen::Matrix3d so3transform(const Eigen::Vector3d& body_z, const ::Eigen::Vector3d& heading, const bool& preserve_heading) {
+      72             : 
+      73       70728 :   Eigen::Vector3d body_z_normed = body_z.normalized();
+      74             : 
+      75       70728 :   Eigen::Matrix3d Rd;
+      76             : 
+      77       70728 :   if (preserve_heading) {
+      78             : 
+      79       23100 :     ROS_DEBUG_THROTTLE(1.0, "[SO3Transform]: using Baca's method");
+      80             : 
+      81             :     // | ------------------------- body z ------------------------- |
+      82       23100 :     Rd.col(2) = body_z_normed;
+      83             : 
+      84             :     // | ------------------------- body x ------------------------- |
+      85             : 
+      86             :     // construct the oblique projection
+      87       23100 :     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       46200 :     Eigen::MatrixXd A = Eigen::MatrixXd(3, 2);
+      91       23100 :     A.col(0)          = projector_body_z_compl.col(0);
+      92       23100 :     A.col(1)          = projector_body_z_compl.col(1);
+      93             : 
+      94             :     // create the basis of the projection null-space complement
+      95       46200 :     Eigen::MatrixXd B = Eigen::MatrixXd(3, 2);
+      96       23100 :     B.col(0)          = Eigen::Vector3d(1, 0, 0);
+      97       23100 :     B.col(1)          = Eigen::Vector3d(0, 1, 0);
+      98             : 
+      99             :     // oblique projector to <range_basis>
+     100       46200 :     Eigen::MatrixXd Bt_A               = B.transpose() * A;
+     101       46200 :     Eigen::MatrixXd Bt_A_pseudoinverse = ((Bt_A.transpose() * Bt_A).inverse()) * Bt_A.transpose();
+     102       23100 :     Eigen::MatrixXd oblique_projector  = A * Bt_A_pseudoinverse * B.transpose();
+     103             : 
+     104       23100 :     Rd.col(0) = oblique_projector * heading;
+     105       23100 :     Rd.col(0).normalize();
+     106             : 
+     107             :     // | ------------------------- body y ------------------------- |
+     108             : 
+     109       23100 :     Rd.col(1) = Rd.col(2).cross(Rd.col(0));
+     110       23100 :     Rd.col(1).normalize();
+     111             : 
+     112             :   } else {
+     113             : 
+     114       47628 :     ROS_DEBUG_THROTTLE(1.0, "[SO3Transform]: using Lee's method");
+     115             : 
+     116       47628 :     Rd.col(2) = body_z_normed;
+     117       47628 :     Rd.col(1) = Rd.col(2).cross(heading);
+     118       47628 :     Rd.col(1).normalize();
+     119       47628 :     Rd.col(0) = Rd.col(1).cross(Rd.col(2));
+     120       47628 :     Rd.col(0).normalize();
+     121             :   }
+     122             : 
+     123      141456 :   return Rd;
+     124             : }
+     125             : 
+     126             : //}
+     127             : 
+     128             : /* getLowestOutput() //{ */
+     129             : 
+     130       76652 : std::optional<CONTROL_OUTPUT> getLowestOuput(const mrs_uav_managers::control_manager::ControlOutputModalities_t& outputs) {
+     131             : 
+     132       76652 :   if (outputs.actuators) {
+     133        2405 :     return ACTUATORS_CMD;
+     134             :   }
+     135             : 
+     136       74247 :   if (outputs.control_group) {
+     137        2434 :     return CONTROL_GROUP;
+     138             :   }
+     139             : 
+     140       71813 :   if (outputs.attitude_rate) {
+     141       63673 :     return ATTITUDE_RATE;
+     142             :   }
+     143             : 
+     144        8140 :   if (outputs.attitude) {
+     145        2216 :     return ATTITUDE;
+     146             :   }
+     147             : 
+     148        5924 :   if (outputs.acceleration_hdg_rate) {
+     149         994 :     return ACCELERATION_HDG_RATE;
+     150             :   }
+     151             : 
+     152        4930 :   if (outputs.acceleration_hdg) {
+     153        1036 :     return ACCELERATION_HDG;
+     154             :   }
+     155             : 
+     156        3894 :   if (outputs.velocity_hdg_rate) {
+     157        2244 :     return VELOCITY_HDG_RATE;
+     158             :   }
+     159             : 
+     160        1650 :   if (outputs.velocity_hdg) {
+     161        1177 :     return VELOCITY_HDG;
+     162             :   }
+     163             : 
+     164         473 :   if (outputs.position) {
+     165         473 :     return POSITION;
+     166             :   }
+     167             : 
+     168           0 :   return {};
+     169             : }
+     170             : 
+     171             : //}
+     172             : 
+     173             : /* getHighestOutput() //{ */
+     174             : 
+     175        9952 : std::optional<CONTROL_OUTPUT> getHighestOuput(const mrs_uav_managers::control_manager::ControlOutputModalities_t& outputs) {
+     176             : 
+     177        9952 :   if (outputs.position) {
+     178           4 :     return POSITION;
+     179             :   }
+     180             : 
+     181        9948 :   if (outputs.velocity_hdg) {
+     182           4 :     return VELOCITY_HDG;
+     183             :   }
+     184             : 
+     185        9944 :   if (outputs.velocity_hdg_rate) {
+     186           9 :     return VELOCITY_HDG_RATE;
+     187             :   }
+     188             : 
+     189        9935 :   if (outputs.acceleration_hdg) {
+     190           6 :     return ACCELERATION_HDG;
+     191             :   }
+     192             : 
+     193        9929 :   if (outputs.acceleration_hdg_rate) {
+     194           3 :     return ACCELERATION_HDG_RATE;
+     195             :   }
+     196             : 
+     197        9926 :   if (outputs.attitude) {
+     198        5681 :     return ATTITUDE;
+     199             :   }
+     200             : 
+     201        4245 :   if (outputs.attitude_rate) {
+     202        1423 :     return ATTITUDE_RATE;
+     203             :   }
+     204             : 
+     205        2822 :   if (outputs.control_group) {
+     206        1411 :     return CONTROL_GROUP;
+     207             :   }
+     208             : 
+     209        1411 :   if (outputs.actuators) {
+     210        1411 :     return ACTUATORS_CMD;
+     211             :   }
+     212             : 
+     213           0 :   return {};
+     214             : }
+     215             : 
+     216             : //}
+     217             : 
+     218             : /* extractThrottle() //{ */
+     219             : 
+     220         199 : std::optional<double> extractThrottle(const mrs_uav_managers::Controller::ControlOutput& control_output) {
+     221             : 
+     222         199 :   if (!control_output.control_output) {
+     223         112 :     return {};
+     224             :   }
+     225             : 
+     226         174 :   return std::visit(HwApiCmdExtractThrottleVisitor(), control_output.control_output.value());
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* attitudeController() //{ */
+     232             : 
+     233       72697 : 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       72697 :   Eigen::Matrix3d R  = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+     238       72697 :   Eigen::Matrix3d Rd = mrs_lib::AttitudeConverter(reference.orientation);
+     239             : 
+     240             :   // calculate the orientation error
+     241       72697 :   Eigen::Vector3d E = common::orientationError(R, Rd);
+     242             : 
+     243       72697 :   Eigen::Vector3d rate_feedback = gains.array() * E.array() + ff_rate.array();
+     244             : 
+     245             :   // | ----------- parasitic heading rate compensation ---------- |
+     246             : 
+     247       72697 :   if (parasitic_heading_rate_compensation) {
+     248             : 
+     249       21969 :     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       21969 :     Eigen::Vector3d rp_heading_rate_compensation(0, 0, 0);
+     253             : 
+     254       21969 :     Eigen::Vector3d q_feedback_yawless = rate_feedback;
+     255       21969 :     q_feedback_yawless(2)              = 0;  // nullyfy the effect of the original yaw feedback
+     256             : 
+     257       21969 :     double parasitic_heading_rate = 0;
+     258             : 
+     259             :     try {
+     260       21969 :       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       21969 :       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       21969 :     rate_feedback += rp_heading_rate_compensation;
+     274             :   }
+     275             : 
+     276             :   // | --------------- saturate the attitude rate --------------- |
+     277             : 
+     278       72697 :   if (rate_feedback[0] > rate_saturation[0]) {
+     279           0 :     rate_feedback[0] = rate_saturation[0];
+     280       72697 :   } else if (rate_feedback[0] < -rate_saturation[0]) {
+     281           0 :     rate_feedback[0] = -rate_saturation[0];
+     282             :   }
+     283             : 
+     284       72697 :   if (rate_feedback[1] > rate_saturation[1]) {
+     285           0 :     rate_feedback[1] = rate_saturation[1];
+     286       72697 :   } else if (rate_feedback[1] < -rate_saturation[1]) {
+     287           0 :     rate_feedback[1] = -rate_saturation[1];
+     288             :   }
+     289             : 
+     290       72697 :   if (rate_feedback[2] > rate_saturation[2]) {
+     291           0 :     rate_feedback[2] = rate_saturation[2];
+     292       72697 :   } 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       72697 :   mrs_msgs::HwApiAttitudeRateCmd cmd;
+     299             : 
+     300       72697 :   cmd.stamp = ros::Time::now();
+     301             : 
+     302       72697 :   cmd.body_rate.x = rate_feedback[0];
+     303       72697 :   cmd.body_rate.y = rate_feedback[1];
+     304       72697 :   cmd.body_rate.z = rate_feedback[2];
+     305             : 
+     306       72697 :   cmd.throttle = reference.throttle;
+     307             : 
+     308      145394 :   return cmd;
+     309             : }
+     310             : 
+     311             : //}
+     312             : 
+     313             : /* attitudeRateController() //{ */
+     314             : 
+     315        7625 : std::optional<mrs_msgs::HwApiControlGroupCmd> attitudeRateController(const mrs_msgs::UavState& uav_state, const mrs_msgs::HwApiAttitudeRateCmd& reference,
+     316             :                                                                      const Eigen::Vector3d& gains) {
+     317             : 
+     318        7625 :   Eigen::Vector3d des_rate(reference.body_rate.x, reference.body_rate.y, reference.body_rate.z);
+     319        7625 :   Eigen::Vector3d cur_rate(uav_state.velocity.angular.x, uav_state.velocity.angular.y, uav_state.velocity.angular.z);
+     320             : 
+     321        7625 :   Eigen::Vector3d ctrl_group_action = (des_rate - cur_rate).array() * gains.array();
+     322             : 
+     323        7625 :   mrs_msgs::HwApiControlGroupCmd cmd;
+     324             : 
+     325        7625 :   cmd.stamp = ros::Time::now();
+     326             : 
+     327        7625 :   cmd.throttle = reference.throttle;
+     328             : 
+     329        7625 :   cmd.roll  = ctrl_group_action[0];
+     330        7625 :   cmd.pitch = ctrl_group_action[1];
+     331        7625 :   cmd.yaw   = ctrl_group_action[2];
+     332             : 
+     333       15250 :   return {cmd};
+     334             : }
+     335             : 
+     336             : //}
+     337             : 
+     338             : /* actuatorMixer() //{ */
+     339             : 
+     340        3798 : mrs_msgs::HwApiActuatorCmd actuatorMixer(const mrs_msgs::HwApiControlGroupCmd& ctrl_group_cmd, const Eigen::MatrixXd& mixer) {
+     341             : 
+     342        3798 :   Eigen::Vector4d ctrl_group(ctrl_group_cmd.roll, ctrl_group_cmd.pitch, ctrl_group_cmd.yaw, ctrl_group_cmd.throttle);
+     343             : 
+     344        7596 :   Eigen::VectorXd motors = mixer * ctrl_group;
+     345             : 
+     346        3798 :   double min = motors.minCoeff();
+     347             : 
+     348        3798 :   if (min < 0.0) {
+     349           0 :     motors.array() += abs(min);
+     350             :   }
+     351             : 
+     352        3798 :   double max = motors.maxCoeff();
+     353             : 
+     354        3798 :   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        3798 :   mrs_msgs::HwApiActuatorCmd actuator_msg;
+     373             : 
+     374        3798 :   actuator_msg.stamp = ros::Time::now();
+     375             : 
+     376       18990 :   for (int i = 0; i < motors.size(); i++) {
+     377       15192 :     actuator_msg.motors.push_back(motors[i]);
+     378             :   }
+     379             : 
+     380        7596 :   return actuator_msg;
+     381             : }
+     382             : 
+     383             : //}
+     384             : 
+     385             : }  // namespace common
+     386             : 
+     387             : }  // namespace mrs_uav_controllers
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_controllers/src/common.cpp.gcov.overview.html b/mrs_uav_controllers/src/common.cpp.gcov.overview.html new file mode 100644 index 0000000000..dd0d0db077 --- /dev/null +++ b/mrs_uav_controllers/src/common.cpp.gcov.overview.html @@ -0,0 +1,117 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/common.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_controllers::failsafe_controller::FailsafeController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::failsafe_controller::FailsafeController::resetDisturbanceEstimators()0
mrs_uav_controllers::failsafe_controller::FailsafeController::getHeadingSafely(geometry_msgs::Quaternion_<std::allocator<void> > const&)8
mrs_uav_controllers::failsafe_controller::FailsafeController::activate(mrs_uav_managers::Controller::ControlOutput const&)8
mrs_uav_controllers::failsafe_controller::FailsafeController::deactivate()32
(anonymous namespace)::ProxyExec0::ProxyExec0()82
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>)82
mrs_uav_controllers::failsafe_controller::FailsafeController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)319
mrs_uav_controllers::failsafe_controller::FailsafeController::getStatus()971
mrs_uav_controllers::failsafe_controller::FailsafeController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)9720
mrs_uav_controllers::failsafe_controller::FailsafeController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)111592
+
+
+ + + +
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..8d06ae6d48 --- /dev/null +++ b/mrs_uav_controllers/src/failsafe_controller.cpp.func.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/failsafe_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - failsafe_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13119467.5 %
Date:2024-04-09 22:03:33Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_controllers::failsafe_controller::FailsafeController::deactivate()32
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>)82
mrs_uav_controllers::failsafe_controller::FailsafeController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)9720
mrs_uav_controllers::failsafe_controller::FailsafeController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)319
mrs_uav_controllers::failsafe_controller::FailsafeController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)111592
mrs_uav_controllers::failsafe_controller::FailsafeController::getHeadingSafely(geometry_msgs::Quaternion_<std::allocator<void> > const&)8
mrs_uav_controllers::failsafe_controller::FailsafeController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)0
mrs_uav_controllers::failsafe_controller::FailsafeController::resetDisturbanceEstimators()0
mrs_uav_controllers::failsafe_controller::FailsafeController::activate(mrs_uav_managers::Controller::ControlOutput const&)8
mrs_uav_controllers::failsafe_controller::FailsafeController::getStatus()971
+
+
+ + + +
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..8775fe0d39 --- /dev/null +++ b/mrs_uav_controllers/src/failsafe_controller.cpp.gcov.html @@ -0,0 +1,635 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/failsafe_controller.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - failsafe_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13119467.5 %
Date:2024-04-09 22:03:33Functions: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          82 : 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          82 :   nh_ = nh;
+     117             : 
+     118          82 :   common_handlers_  = common_handlers;
+     119          82 :   private_handlers_ = private_handlers;
+     120             : 
+     121          82 :   _uav_mass_ = common_handlers->getMass();
+     122             : 
+     123          82 :   ros::Time::waitForValid();
+     124             : 
+     125             :   // | ---------- loading params using the parent's nh ---------- |
+     126             : 
+     127         164 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     128             : 
+     129          82 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     130             : 
+     131          82 :   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          82 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/failsafe_controller.yaml");
+     139          82 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/failsafe_controller.yaml");
+     140             : 
+     141         164 :   const std::string yaml_namespace = "mrs_uav_controllers/failsafe_controller/";
+     142             : 
+     143          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "throttle_output/throttle_decrease_rate", _throttle_decrease_rate_);
+     144          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "throttle_output/initial_throttle_percentage", _initial_throttle_percentage_);
+     145             : 
+     146          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "attitude_controller/gains/kp", _kq_);
+     147             : 
+     148          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "rate_controller/gains/kp", _kw_);
+     149             : 
+     150          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "velocity_output/descend_speed", _descend_speed_);
+     151          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "acceleration_output/descend_acceleration", _descend_acceleration_);
+     152             : 
+     153          82 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     154           0 :     ROS_ERROR("[FailsafeController]: Could not load all parameters!");
+     155           0 :     return false;
+     156             :   }
+     157             : 
+     158          82 :   _descend_speed_        = std::abs(_descend_speed_);
+     159          82 :   _descend_acceleration_ = std::abs(_descend_acceleration_);
+     160             : 
+     161          82 :   uav_mass_difference_ = 0;
+     162             : 
+     163             :   // | ----------------------- subscribers ---------------------- |
+     164             : 
+     165          82 :   mrs_lib::SubscribeHandlerOptions shopts;
+     166          82 :   shopts.nh                 = nh_;
+     167          82 :   shopts.node_name          = "FailsafeController";
+     168          82 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     169          82 :   shopts.threadsafe         = true;
+     170          82 :   shopts.autostart          = true;
+     171          82 :   shopts.queue_size         = 10;
+     172          82 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     173             : 
+     174          82 :   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          82 :   hover_throttle_ = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, _uav_mass_ * common_handlers_->g);
+     179             : 
+     180             :   // | ------------------------ profiler ------------------------ |
+     181             : 
+     182          82 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "FailsafeController", _profiler_enabled_);
+     183             : 
+     184             :   // | ----------------------- finish init ---------------------- |
+     185             : 
+     186          82 :   ROS_INFO("[FailsafeController]: initialized");
+     187             : 
+     188          82 :   is_initialized_ = true;
+     189             : 
+     190          82 :   return true;
+     191             : }
+     192             : 
+     193             : //}
+     194             : 
+     195             : /* activate() //{ */
+     196             : 
+     197           8 : bool FailsafeController::activate(const ControlOutput &last_control_output) {
+     198             : 
+     199          16 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     200             : 
+     201           8 :   if (!last_control_output.control_output) {
+     202             : 
+     203           0 :     ROS_WARN("[FailsafeController]: activated without getting the last controller's command");
+     204             : 
+     205           0 :     return false;
+     206             : 
+     207             :   } else {
+     208             : 
+     209             :     // | -------------- calculate the initial heading ------------- |
+     210             : 
+     211           8 :     if (sh_hw_api_orientation_.getMsg()) {
+     212             : 
+     213          16 :       auto hw_api_orientation = sh_hw_api_orientation_.getMsg();
+     214             : 
+     215           8 :       heading_setpoint_ = getHeadingSafely(hw_api_orientation->quaternion);
+     216             : 
+     217           8 :       ROS_INFO("[FailsafeController]: activated with heading = %.2f rad", heading_setpoint_);
+     218             : 
+     219             :     } else {
+     220             : 
+     221           0 :       ROS_ERROR("[FailsafeController]: missing orientation from HW API, activated with heading = 0 rad");
+     222             : 
+     223           0 :       heading_setpoint_ = 0;
+     224             :     }
+     225             : 
+     226           8 :     activation_control_output_ = last_control_output;
+     227             : 
+     228           8 :     if (last_control_output.diagnostics.mass_estimator) {
+     229           8 :       uav_mass_difference_ = last_control_output.diagnostics.mass_difference;
+     230             :     } else {
+     231           0 :       uav_mass_difference_ = 0;
+     232             :     }
+     233             : 
+     234           8 :     activation_control_output_.diagnostics.controller_enforcing_constraints = false;
+     235             : 
+     236           8 :     hover_throttle_ = _initial_throttle_percentage_ * mrs_lib::quadratic_throttle_model::forceToThrottle(
+     237           8 :                                                           common_handlers_->throttle_model, (_uav_mass_ + uav_mass_difference_) * common_handlers_->g);
+     238             : 
+     239           8 :     ROS_INFO("[FailsafeController]: activated with uav_mass_difference %.2f kg.", uav_mass_difference_);
+     240             :   }
+     241             : 
+     242           8 :   first_iteration_ = true;
+     243             : 
+     244           8 :   is_active_ = true;
+     245             : 
+     246           8 :   return true;
+     247             : }
+     248             : 
+     249             : //}
+     250             : 
+     251             : /* deactivate() //{ */
+     252             : 
+     253          32 : void FailsafeController::deactivate(void) {
+     254             : 
+     255          32 :   is_active_           = false;
+     256          32 :   first_iteration_     = false;
+     257          32 :   uav_mass_difference_ = 0;
+     258             : 
+     259          32 :   ROS_INFO("[FailsafeController]: deactivated");
+     260          32 : }
+     261             : 
+     262             : //}
+     263             : 
+     264             : /* updateInactive() //{ */
+     265             : 
+     266      111592 : void FailsafeController::updateInactive(const mrs_msgs::UavState &uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &tracker_command) {
+     267             : 
+     268      111592 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     269             : 
+     270      111592 :   last_update_time_ = ros::Time::now();
+     271             : 
+     272      111592 :   first_iteration_ = false;
+     273      111592 : }
+     274             : 
+     275             : //}
+     276             : 
+     277             : /* //{ updateWhenAcctive() */
+     278             : 
+     279        9720 : FailsafeController::ControlOutput FailsafeController::updateActive(const mrs_msgs::UavState &                       uav_state,
+     280             :                                                                    [[maybe_unused]] const mrs_msgs::TrackerCommand &tracker_command) {
+     281             : 
+     282       29160 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     283       29160 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("FailsafeController::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     284             : 
+     285             :   {
+     286       19440 :     std::scoped_lock lock(mutex_uav_state_);
+     287             : 
+     288        9720 :     uav_state_ = uav_state;
+     289             :   }
+     290             : 
+     291        9720 :   if (!is_active_) {
+     292           0 :     return ControlOutput();
+     293             :   }
+     294             : 
+     295        9720 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     296             : 
+     297             :   // | -------------------- calculate the dt -------------------- |
+     298             : 
+     299             :   double dt;
+     300             : 
+     301        9720 :   if (first_iteration_) {
+     302           8 :     dt               = 0.01;
+     303           8 :     first_iteration_ = false;
+     304             :   } else {
+     305        9712 :     dt = (ros::Time::now() - last_update_time_).toSec();
+     306             :   }
+     307             : 
+     308        9720 :   last_update_time_ = ros::Time::now();
+     309             : 
+     310        9720 :   hover_throttle_ -= _throttle_decrease_rate_ * dt;
+     311             : 
+     312        9720 :   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        9720 :   } else if (hover_throttle_ > 1.0) {
+     316           0 :     hover_throttle_ = 1.0;
+     317        9720 :   } else if (hover_throttle_ < 0.0) {
+     318           0 :     hover_throttle_ = 0.0;
+     319             :   }
+     320             : 
+     321             :   // | --------------- prepare the control output --------------- |
+     322             : 
+     323       19440 :   FailsafeController::ControlOutput control_output;
+     324             : 
+     325        9720 :   control_output.diagnostics.controller = "FailsafeController";
+     326             : 
+     327        9720 :   auto highest_modality = common::getHighestOuput(common_handlers_->control_output_modalities);
+     328             : 
+     329        9720 :   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        9720 :   control_output.diagnostics.controller = "FailsafeController";
+     335             : 
+     336             :   // --------------------------------------------------------------
+     337             :   // |                       position output                      |
+     338             :   // --------------------------------------------------------------
+     339             : 
+     340        9720 :   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        9720 :   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        9720 :   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        9720 :   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        9720 :   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        9720 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+     422             : 
+     423        9720 :   attitude_cmd.stamp       = ros::Time::now();
+     424        9720 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(0, 0, heading_setpoint_);
+     425        9720 :   attitude_cmd.throttle    = hover_throttle_;
+     426             : 
+     427        9720 :   if (highest_modality.value() == common::ATTITUDE) {
+     428        5535 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning attitude output");
+     429        5535 :     control_output.control_output = attitude_cmd;
+     430        5535 :     return control_output;
+     431             :   }
+     432             : 
+     433             :   // --------------------------------------------------------------
+     434             :   // |                      attitude control                      |
+     435             :   // --------------------------------------------------------------
+     436             : 
+     437        4185 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+     438        4185 :   Eigen::Vector3d rate_ff(0, 0, 0);
+     439        4185 :   Eigen::Vector3d Kq(_kq_, _kq_, _kq_);
+     440             : 
+     441        4185 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, rate_ff, attitude_rate_saturation, Kq, false);
+     442             : 
+     443        4185 :   if (highest_modality.value() == common::ATTITUDE_RATE) {
+     444        1399 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning attitude rate output");
+     445        1399 :     control_output.control_output = attitude_rate_command;
+     446        1399 :     return control_output;
+     447             :   }
+     448             : 
+     449             :   // --------------------------------------------------------------
+     450             :   // |                    attitude rate control                   |
+     451             :   // --------------------------------------------------------------
+     452             : 
+     453        2786 :   Eigen::Vector3d Kw = common_handlers_->detailed_model_params->inertia.diagonal() * _kw_;
+     454             : 
+     455        2786 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+     456             : 
+     457        2786 :   if (highest_modality.value() == common::CONTROL_GROUP) {
+     458        1393 :     ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning control group output");
+     459        1393 :     control_output.control_output = control_group_command;
+     460        1393 :     return control_output;
+     461             :   }
+     462             : 
+     463             :   // --------------------------------------------------------------
+     464             :   // |                            mixer                           |
+     465             :   // --------------------------------------------------------------
+     466             : 
+     467        2786 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+     468             : 
+     469        1393 :   ROS_DEBUG_THROTTLE(1.0, "[FailsafeController]: returning actuators output");
+     470        1393 :   control_output.control_output = actuator_cmd;
+     471        1393 :   return control_output;
+     472             : }
+     473             : 
+     474             : //}
+     475             : 
+     476             : /* getStatus() //{ */
+     477             : 
+     478         971 : const mrs_msgs::ControllerStatus FailsafeController::getStatus() {
+     479             : 
+     480         971 :   mrs_msgs::ControllerStatus controller_status;
+     481             : 
+     482         971 :   controller_status.active = is_active_;
+     483             : 
+     484         971 :   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         319 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr FailsafeController::setConstraints([
+     506             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) {
+     507             : 
+     508         319 :   if (!is_initialized_) {
+     509           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     510             :   }
+     511             : 
+     512         319 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     513             : 
+     514         319 :   ROS_INFO("[FailsafeController]: updating constraints");
+     515             : 
+     516         638 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     517         319 :   res.success = true;
+     518         319 :   res.message = "constraints updated";
+     519             : 
+     520         319 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     521             : }
+     522             : 
+     523             : //}
+     524             : 
+     525             : /* getHeadingSafely() //{ */
+     526             : 
+     527           8 : double FailsafeController::getHeadingSafely(const geometry_msgs::Quaternion &quaternion) {
+     528             : 
+     529             :   try {
+     530           8 :     return mrs_lib::AttitudeConverter(quaternion).getHeading();
+     531             :   }
+     532           0 :   catch (...) {
+     533             :   }
+     534             : 
+     535             :   try {
+     536           0 :     return mrs_lib::AttitudeConverter(quaternion).getYaw();
+     537             :   }
+     538           0 :   catch (...) {
+     539             :   }
+     540             : 
+     541           0 :   return 0;
+     542             : }
+     543             : 
+     544             : //}
+     545             : 
+     546             : }  // namespace failsafe_controller
+     547             : 
+     548             : }  // namespace mrs_uav_controllers
+     549             : 
+     550             : #include <pluginlib/class_list_macros.h>
+     551          82 : 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..e0531bfc0f --- /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:1760217281.0 %
Date:2024-04-09 22:03:33Functions: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
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
<unnamed>88.7 %134 / 15181.8 %9 / 11
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 %624 / 77488.2 %15 / 17
<unnamed>80.6 %624 / 77488.2 %15 / 17
mpc_controller.cpp +
82.4%82.4%
+ 		82.4 %737 / 89488.9 %16 / 18
<unnamed>82.4 %737 / 89488.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..c2d38e2cf1 --- /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:1760217281.0 %
Date:2024-04-09 22:03:33Functions: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 %624 / 77488.2 %15 / 17
<unnamed>80.6 %624 / 77488.2 %15 / 17
mpc_controller.cpp +
82.4%82.4%
+ 		82.4 %737 / 89488.9 %16 / 18
<unnamed>82.4 %737 / 89488.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..2dadcb61eb --- /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:1760217281.0 %
Date:2024-04-09 22:03:33Functions: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 +
82.4%82.4%
+ 		82.4 %737 / 89488.9 %16 / 18
<unnamed>82.4 %737 / 89488.9 %16 / 18
se3_controller.cpp +
80.6%80.6%
+ 		80.6 %624 / 77488.2 %15 / 17
<unnamed>80.6 %624 / 77488.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..dac601cabe --- /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:1760217281.0 %
Date:2024-04-09 22:03:33Functions: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
midair_activation_controller.cpp +
88.7%88.7%
+ 		88.7 %134 / 15181.8 %9 / 11
failsafe_controller.cpp +
67.5%67.5%
+ 		67.5 %131 / 19481.8 %9 / 11
se3_controller.cpp +
80.6%80.6%
+ 		80.6 %624 / 77488.2 %15 / 17
mpc_controller.cpp +
82.4%82.4%
+ 		82.4 %737 / 89488.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..79973e7c62 --- /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:1760217281.0 %
Date:2024-04-09 22:03:33Functions: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 %624 / 77488.2 %15 / 17
mpc_controller.cpp +
82.4%82.4%
+ 		82.4 %737 / 89488.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..6de3fba7f7 --- /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:1760217281.0 %
Date:2024-04-09 22:03:33Functions: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 +
82.4%82.4%
+ 		82.4 %737 / 89488.9 %16 / 18
se3_controller.cpp +
80.6%80.6%
+ 		80.6 %624 / 77488.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..b9c1fbaa9a --- /dev/null +++ b/mrs_uav_controllers/src/midair_activation_controller.cpp.func-sort-c.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/midair_activation_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - midair_activation_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415188.7 %
Date:2024-04-09 22:03:33Functions: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&)14
mrs_uav_controllers::midair_activation_controller::MidairActivationController::deactivate()76
(anonymous namespace)::ProxyExec0::ProxyExec0()82
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>)82
mrs_uav_controllers::midair_activation_controller::MidairActivationController::getStatus()95
mrs_uav_controllers::midair_activation_controller::MidairActivationController::activate(mrs_uav_managers::Controller::ControlOutput const&)113
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)232
mrs_uav_controllers::midair_activation_controller::MidairActivationController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)319
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)121080
+
+
+ + + +
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..2ffa9ff2c6 --- /dev/null +++ b/mrs_uav_controllers/src/midair_activation_controller.cpp.func.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/midair_activation_controller.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - midair_activation_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415188.7 %
Date:2024-04-09 22:03:33Functions:91181.8 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_controllers::midair_activation_controller::MidairActivationController::deactivate()76
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>)82
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)232
mrs_uav_controllers::midair_activation_controller::MidairActivationController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)319
mrs_uav_controllers::midair_activation_controller::MidairActivationController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)121080
mrs_uav_controllers::midair_activation_controller::MidairActivationController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)14
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&)113
mrs_uav_controllers::midair_activation_controller::MidairActivationController::getStatus()95
+
+
+ + + +
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..550ee98d65 --- /dev/null +++ b/mrs_uav_controllers/src/midair_activation_controller.cpp.gcov.html @@ -0,0 +1,518 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_controllers/src/midair_activation_controller.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_controllers/src - midair_activation_controller.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13415188.7 %
Date:2024-04-09 22:03:33Functions: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          82 : 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          82 :   nh_ = nh;
+      90             : 
+      91          82 :   common_handlers_  = common_handlers;
+      92          82 :   private_handlers_ = private_handlers;
+      93             : 
+      94          82 :   _uav_mass_ = common_handlers->getMass();
+      95             : 
+      96          82 :   ros::Time::waitForValid();
+      97             : 
+      98             :   // | ---------- loading params using the parent's nh ---------- |
+      99             : 
+     100         164 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     101             : 
+     102          82 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     103             : 
+     104          82 :   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          82 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/midair_activation_controller.yaml");
+     112          82 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/midair_activation_controller.yaml");
+     113             : 
+     114          82 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     115           0 :     ROS_ERROR("[MidairActivationController]: Could not load all parameters!");
+     116           0 :     return false;
+     117             :   }
+     118             : 
+     119          82 :   uav_mass_difference_ = 0;
+     120             : 
+     121             :   // | ------------------------ profiler ------------------------ |
+     122             : 
+     123          82 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "MidairActivationController", _profiler_enabled_);
+     124             : 
+     125             :   // | ----------------------- finish init ---------------------- |
+     126             : 
+     127          82 :   ROS_INFO("[MidairActivationController]: initialized");
+     128             : 
+     129          82 :   is_initialized_ = true;
+     130             : 
+     131          82 :   return true;
+     132             : }
+     133             : 
+     134             : //}
+     135             : 
+     136             : /* activate() //{ */
+     137             : 
+     138         113 : bool MidairActivationController::activate([[maybe_unused]] const ControlOutput &last_control_output) {
+     139             : 
+     140         113 :   ROS_INFO("[MidairActivationController]: activating");
+     141             : 
+     142         113 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     143             : 
+     144         113 :   hover_throttle_ = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, _uav_mass_ * common_handlers_->g);
+     145             : 
+     146         113 :   is_active_ = true;
+     147             : 
+     148         113 :   ROS_INFO("[MidairActivationController]: activated, hover throttle %.2f", hover_throttle_);
+     149             : 
+     150         226 :   return true;
+     151             : }
+     152             : 
+     153             : //}
+     154             : 
+     155             : /* deactivate() //{ */
+     156             : 
+     157          76 : void MidairActivationController::deactivate(void) {
+     158             : 
+     159          76 :   is_active_           = false;
+     160          76 :   uav_mass_difference_ = 0;
+     161             : 
+     162          76 :   ROS_INFO("[MidairActivationController]: deactivated");
+     163          76 : }
+     164             : 
+     165             : //}
+     166             : 
+     167             : /* updateInactive() //{ */
+     168             : 
+     169      121080 : void MidairActivationController::updateInactive(const mrs_msgs::UavState &                                      uav_state,
+     170             :                                                 [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &tracker_command) {
+     171             : 
+     172      121080 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     173      121080 : }
+     174             : 
+     175             : //}
+     176             : 
+     177             : /* //{ updateWhenAcctive() */
+     178             : 
+     179         232 : MidairActivationController::ControlOutput MidairActivationController::updateActive(const mrs_msgs::UavState &      uav_state,
+     180             :                                                                                    const mrs_msgs::TrackerCommand &tracker_command) {
+     181             : 
+     182         696 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     183             :   mrs_lib::ScopeTimer timer =
+     184         696 :       mrs_lib::ScopeTimer("MidairActivationController::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     185             : 
+     186         232 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     187             : 
+     188         232 :   if (!is_active_) {
+     189           0 :     return Controller::ControlOutput();
+     190             :   }
+     191             : 
+     192             :   // | --------------- prepare the control output --------------- |
+     193             : 
+     194         464 :   ControlOutput control_output;
+     195             : 
+     196         232 :   control_output.diagnostics.controller = "MidairActivationController";
+     197             : 
+     198         232 :   auto highest_modality = common::getHighestOuput(common_handlers_->control_output_modalities);
+     199             : 
+     200         232 :   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         232 :   switch (highest_modality.value()) {
+     208             : 
+     209           4 :     case common::POSITION: {
+     210             : 
+     211           8 :       mrs_msgs::HwApiPositionCmd cmd;
+     212             : 
+     213           4 :       cmd.header.stamp    = ros::Time::now();
+     214           4 :       cmd.header.frame_id = uav_state.header.frame_id;
+     215             : 
+     216           4 :       cmd.position.x = uav_state.pose.position.x;
+     217           4 :       cmd.position.y = uav_state.pose.position.y;
+     218           4 :       cmd.position.z = uav_state.pose.position.z;
+     219             : 
+     220           4 :       cmd.heading = getHeadingSafely(uav_state, tracker_command);
+     221             : 
+     222           4 :       control_output.control_output = cmd;
+     223             : 
+     224           4 :       break;
+     225             :     }
+     226             : 
+     227           4 :     case common::VELOCITY_HDG: {
+     228             : 
+     229           8 :       mrs_msgs::HwApiVelocityHdgCmd cmd;
+     230             : 
+     231           4 :       cmd.header.stamp    = ros::Time::now();
+     232           4 :       cmd.header.frame_id = uav_state.header.frame_id;
+     233             : 
+     234           4 :       cmd.velocity.x = uav_state.velocity.linear.x;
+     235           4 :       cmd.velocity.y = uav_state.velocity.linear.y;
+     236           4 :       cmd.velocity.z = uav_state.velocity.linear.z;
+     237             : 
+     238           4 :       cmd.heading = getHeadingSafely(uav_state, tracker_command);
+     239             : 
+     240           4 :       control_output.control_output = cmd;
+     241             : 
+     242           4 :       break;
+     243             :     }
+     244             : 
+     245           9 :     case common::VELOCITY_HDG_RATE: {
+     246             : 
+     247          18 :       mrs_msgs::HwApiVelocityHdgRateCmd cmd;
+     248             : 
+     249           9 :       cmd.header.stamp    = ros::Time::now();
+     250           9 :       cmd.header.frame_id = uav_state.header.frame_id;
+     251             : 
+     252           9 :       cmd.velocity.x = uav_state.velocity.linear.x;
+     253           9 :       cmd.velocity.y = uav_state.velocity.linear.y;
+     254           9 :       cmd.velocity.z = uav_state.velocity.linear.z;
+     255             : 
+     256           9 :       cmd.heading_rate = 0;
+     257             : 
+     258           9 :       control_output.control_output = cmd;
+     259             : 
+     260           9 :       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           3 :     case common::ACCELERATION_HDG_RATE: {
+     282             : 
+     283           6 :       mrs_msgs::HwApiAccelerationHdgRateCmd cmd;
+     284             : 
+     285           3 :       cmd.header.stamp    = ros::Time::now();
+     286           3 :       cmd.header.frame_id = uav_state.header.frame_id;
+     287             : 
+     288           3 :       cmd.acceleration.x = uav_state.acceleration.linear.x;
+     289           3 :       cmd.acceleration.y = uav_state.acceleration.linear.y;
+     290           3 :       cmd.acceleration.z = uav_state.acceleration.linear.z;
+     291             : 
+     292           3 :       cmd.heading_rate = 0;
+     293             : 
+     294           3 :       control_output.control_output = cmd;
+     295             : 
+     296           3 :       break;
+     297             :     }
+     298             : 
+     299         146 :     case common::ATTITUDE: {
+     300             : 
+     301         146 :       mrs_msgs::HwApiAttitudeCmd cmd;
+     302             : 
+     303         146 :       cmd.stamp = ros::Time::now();
+     304             : 
+     305         146 :       cmd.orientation = uav_state.pose.orientation;
+     306         146 :       cmd.throttle    = hover_throttle_;
+     307             : 
+     308         146 :       control_output.control_output = cmd;
+     309             : 
+     310         146 :       break;
+     311             :     }
+     312             : 
+     313          24 :     case common::ATTITUDE_RATE: {
+     314             : 
+     315          24 :       mrs_msgs::HwApiAttitudeRateCmd cmd;
+     316             : 
+     317          24 :       cmd.stamp = ros::Time::now();
+     318             : 
+     319          24 :       cmd.body_rate.x = 0;
+     320          24 :       cmd.body_rate.y = 0;
+     321          24 :       cmd.body_rate.z = 0;
+     322             : 
+     323          24 :       cmd.throttle = hover_throttle_;
+     324             : 
+     325          24 :       control_output.control_output = cmd;
+     326             : 
+     327          24 :       break;
+     328             :     }
+     329             : 
+     330          18 :     case common::CONTROL_GROUP: {
+     331             : 
+     332          18 :       mrs_msgs::HwApiControlGroupCmd cmd;
+     333             : 
+     334          18 :       cmd.stamp = ros::Time::now();
+     335             : 
+     336          18 :       cmd.roll     = 0;
+     337          18 :       cmd.pitch    = 0;
+     338          18 :       cmd.yaw      = 0;
+     339          18 :       cmd.throttle = hover_throttle_;
+     340             : 
+     341          18 :       control_output.control_output = cmd;
+     342             : 
+     343          18 :       break;
+     344             :     }
+     345             : 
+     346          18 :     case common::ACTUATORS_CMD: {
+     347             : 
+     348          36 :       mrs_msgs::HwApiActuatorCmd cmd;
+     349             : 
+     350          18 :       cmd.stamp = ros::Time::now();
+     351             : 
+     352          90 :       for (int i = 0; i < common_handlers_->throttle_model.n_motors; i++) {
+     353          72 :         cmd.motors.push_back(hover_throttle_);
+     354             :       }
+     355             : 
+     356          18 :       control_output.control_output = cmd;
+     357             : 
+     358          18 :       break;
+     359             :     }
+     360             :   }
+     361             : 
+     362         232 :   return control_output;
+     363             : }  // namespace midair_activation_controller
+     364             : 
+     365             : //}
+     366             : 
+     367             : /* getStatus() //{ */
+     368             : 
+     369          95 : const mrs_msgs::ControllerStatus MidairActivationController::getStatus() {
+     370             : 
+     371          95 :   mrs_msgs::ControllerStatus controller_status;
+     372             : 
+     373          95 :   controller_status.active = is_active_;
+     374             : 
+     375          95 :   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         319 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MidairActivationController::setConstraints([
+     397             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) {
+     398             : 
+     399         319 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     400             : }
+     401             : 
+     402             : //}
+     403             : 
+     404             : /* getHeadingSafely() //{ */
+     405             : 
+     406          14 : double MidairActivationController::getHeadingSafely(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+     407             : 
+     408             :   try {
+     409          14 :     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          82 : 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:73789482.4 %
Date:2024-04-09 22:03:33Functions: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&)4
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_controllers::mpc_controller::MpcController::deactivate()138
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>)164
mrs_uav_controllers::mpc_controller::MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig&, unsigned int)164
mrs_uav_controllers::mpc_controller::MpcController::activate(mrs_uav_managers::Controller::ControlOutput const&)172
mrs_uav_controllers::mpc_controller::MpcController::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)250
mrs_uav_controllers::mpc_controller::MpcController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)638
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&)745
mrs_uav_controllers::mpc_controller::MpcController::getStatus()11296
mrs_uav_controllers::mpc_controller::MpcController::timerGains(ros::TimerEvent const&)12536
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&)48664
mrs_uav_controllers::mpc_controller::MpcController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)49409
mrs_uav_controllers::mpc_controller::MpcController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)49659
mrs_uav_controllers::mpc_controller::MpcController::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)100288
mrs_uav_controllers::mpc_controller::MpcController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)192965
+
+
+ + + +
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..739dcb81ea --- /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:73789482.4 %
Date:2024-04-09 22:03:33Functions:161888.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_controllers::mpc_controller::MpcController::deactivate()138
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>)164
mrs_uav_controllers::mpc_controller::MpcController::timerGains(ros::TimerEvent const&)12536
mrs_uav_controllers::mpc_controller::MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig&, unsigned int)164
mrs_uav_controllers::mpc_controller::MpcController::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)49659
mrs_uav_controllers::mpc_controller::MpcController::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)638
mrs_uav_controllers::mpc_controller::MpcController::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)192965
mrs_uav_controllers::mpc_controller::MpcController::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)49409
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&)745
mrs_uav_controllers::mpc_controller::MpcController::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)100288
mrs_uav_controllers::mpc_controller::MpcController::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)250
mrs_uav_controllers::mpc_controller::MpcController::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)4
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&)48664
mrs_uav_controllers::mpc_controller::MpcController::activate(mrs_uav_managers::Controller::ControlOutput const&)172
mrs_uav_controllers::mpc_controller::MpcController::getStatus()11296
+
+
+ + + +
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..78f096bdbb --- /dev/null +++ b/mrs_uav_controllers/src/mpc_controller.cpp.gcov.html @@ -0,0 +1,2214 @@ + + + + + + + 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:73789482.4 %
Date:2024-04-09 22:03:33Functions: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         164 : 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         164 :   nh_ = nh;
+     260             : 
+     261         164 :   common_handlers_  = common_handlers;
+     262         164 :   private_handlers_ = private_handlers;
+     263             : 
+     264         164 :   _uav_mass_ = common_handlers_->getMass();
+     265         164 :   name_      = private_handlers_->runtime_name;
+     266             : 
+     267         164 :   ros::Time::waitForValid();
+     268             : 
+     269             :   // | ---------- loading params using the parent's nh ---------- |
+     270             : 
+     271         328 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     272             : 
+     273         164 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     274             : 
+     275         164 :   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         164 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/mpc_controller.yaml");
+     283         164 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/mpc_controller.yaml");
+     284         164 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/" + private_handlers->name_space + ".yaml");
+     285         164 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/" + private_handlers->name_space + ".yaml");
+     286             : 
+     287         328 :   const std::string yaml_namespace = "mrs_uav_controllers/" + private_handlers_->name_space + "/";
+     288             : 
+     289             :   // load the dynamicall model parameters
+     290         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_model/number_of_states", _n_states_);
+     291         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_model/dt1", _dt1_);
+     292         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_model/dt2", _dt2_);
+     293             : 
+     294         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizon_length", _horizon_length_);
+     295             : 
+     296         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/max_speed", _max_speed_horizontal_);
+     297         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/max_acceleration", _max_acceleration_horizontal_);
+     298         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/max_jerk", _max_jerk_);
+     299             : 
+     300         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/Q", _mat_Q_);
+     301         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/horizontal/S", _mat_S_);
+     302             : 
+     303         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/max_speed", _max_speed_vertical_);
+     304         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/max_acceleration", _max_acceleration_vertical_);
+     305         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/max_u", _max_u_vertical_);
+     306             : 
+     307         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/Q", _mat_Q_z_);
+     308         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/mpc_parameters/vertical/S", _mat_S_z_);
+     309             : 
+     310         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/solver/verbose", _mpc_solver_verbose_);
+     311         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "mpc/solver/max_iterations", _mpc_solver_max_iterations_);
+     312             : 
+     313             :   // | ------------------------- rampup ------------------------- |
+     314             : 
+     315         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/rampup/enabled", _rampup_enabled_);
+     316         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/rampup/speed", _rampup_speed_);
+     317             : 
+     318             :   // | --------------------- integral gains --------------------- |
+     319             : 
+     320         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/integral_gains/kiw", gains_.kiwxy);
+     321         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/integral_gains/kib", gains_.kibxy);
+     322             : 
+     323             :   // integrator limits
+     324         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/integral_gains/kiw_lim", gains_.kiwxy_lim);
+     325         164 :   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         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude/roll_pitch", gains_.kq_roll_pitch);
+     331         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude/yaw", gains_.kq_yaw);
+     332             : 
+     333             :   // attitude rate gains
+     334         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude_rate/roll_pitch", gains_.kw_rp);
+     335         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gains/attitude_rate/yaw", gains_.kw_y);
+     336             : 
+     337             :   // mass estimator
+     338         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/mass_estimator/km", gains_.km);
+     339         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/mass_estimator/km_lim", gains_.km_lim);
+     340             : 
+     341             :   // constraints
+     342         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/constraints/tilt_angle_failsafe/enabled", _tilt_angle_failsafe_enabled_);
+     343         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/constraints/tilt_angle_failsafe/limit", _tilt_angle_failsafe_);
+     344             : 
+     345         164 :   _tilt_angle_failsafe_ = M_PI * (_tilt_angle_failsafe_ / 180.0);
+     346             : 
+     347         164 :   if (_tilt_angle_failsafe_enabled_ && fabs(_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         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/constraints/throttle_saturation", _throttle_saturation_);
+     353             : 
+     354             :   // gain filtering
+     355         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/perc_change_rate", _gains_filter_change_rate_);
+     356         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/min_change_rate", _gains_filter_min_change_rate_);
+     357         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/rate", _gain_filtering_rate_);
+     358         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/gain_filtering/gain_mute_coefficient", _gain_mute_coefficient_);
+     359             : 
+     360             :   // angular rate feed forward
+     361         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/angular_rate_feedforward/jerk", drs_params_.jerk_feedforward);
+     362             : 
+     363             :   // output mode
+     364         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "so3/preferred_output", drs_params_.preferred_output_mode);
+     365             : 
+     366             :   // | ------------------- position pid params ------------------ |
+     367             : 
+     368         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/p", _pos_pid_p_);
+     369         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/i", _pos_pid_i_);
+     370         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/d", _pos_pid_d_);
+     371             : 
+     372         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/p", _hdg_pid_p_);
+     373         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/i", _hdg_pid_i_);
+     374         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/d", _hdg_pid_d_);
+     375             : 
+     376         164 :   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         164 :   if (!(drs_params_.preferred_output_mode == OUTPUT_ACTUATORS || drs_params_.preferred_output_mode == OUTPUT_CONTROL_GROUP ||
+     384         164 :         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         164 :   uav_mass_difference_ = 0;
+     390         164 :   Iw_w_                = Eigen::Vector2d::Zero(2);
+     391         164 :   Ib_b_                = Eigen::Vector2d::Zero(2);
+     392             : 
+     393             :   // | ----------------- prepare the MPC solver ----------------- |
+     394             : 
+     395         164 :   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         164 :                                                                             _dt2_, 0, 1.0);
+     397         164 :   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         164 :                                                                             _dt2_, 0, 1.0);
+     399         164 :   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         164 :                                                                             _dt1_, _dt2_, 0.5, 0.5);
+     401             : 
+     402             :   // | --------------- dynamic reconfigure server --------------- |
+     403             : 
+     404         164 :   drs_params_.kiwxy         = gains_.kiwxy;
+     405         164 :   drs_params_.kibxy         = gains_.kibxy;
+     406         164 :   drs_params_.kq_roll_pitch = gains_.kq_roll_pitch;
+     407         164 :   drs_params_.kq_yaw        = gains_.kq_yaw;
+     408         164 :   drs_params_.km            = gains_.km;
+     409         164 :   drs_params_.km_lim        = gains_.km_lim;
+     410         164 :   drs_params_.kiwxy_lim     = gains_.kiwxy_lim;
+     411         164 :   drs_params_.kibxy_lim     = gains_.kibxy_lim;
+     412             : 
+     413         164 :   drs_.reset(new Drs_t(mutex_drs_, nh_));
+     414         164 :   drs_->updateConfig(drs_params_);
+     415         164 :   Drs_t::CallbackType f = boost::bind(&MpcController::callbackDrs, this, _1, _2);
+     416         164 :   drs_->setCallback(f);
+     417             : 
+     418             :   // | --------------------- service servers -------------------- |
+     419             : 
+     420         164 :   service_set_integral_terms_ = nh_.advertiseService("set_integral_terms_in", &MpcController::callbackSetIntegralTerms, this);
+     421             : 
+     422             :   // | ------------------------- timers ------------------------- |
+     423             : 
+     424         164 :   timer_gains_ = nh_.createTimer(ros::Rate(_gain_filtering_rate_), &MpcController::timerGains, this, false, false);
+     425             : 
+     426             :   // | ---------------------- position pid ---------------------- |
+     427             : 
+     428         164 :   position_pid_x_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     429         164 :   position_pid_y_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     430         164 :   position_pid_z_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     431         164 :   position_pid_heading_.setParams(_hdg_pid_p_, _hdg_pid_d_, _hdg_pid_i_, -1, 0.1);
+     432             : 
+     433             :   // | ------------------------ profiler ------------------------ |
+     434             : 
+     435         164 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "MpcController", _profiler_enabled_);
+     436             : 
+     437             :   // | ----------------------- finish init ---------------------- |
+     438             : 
+     439         164 :   ROS_INFO("[%s]: initialized", this->name_.c_str());
+     440             : 
+     441         164 :   is_initialized_ = true;
+     442             : 
+     443         164 :   return true;
+     444             : }
+     445             : 
+     446             : //}
+     447             : 
+     448             : /* //{ activate() */
+     449             : 
+     450         172 : bool MpcController::activate(const ControlOutput &last_control_output) {
+     451             : 
+     452         172 :   activation_control_output_ = last_control_output;
+     453             : 
+     454         172 :   double activation_mass = _uav_mass_;
+     455             : 
+     456         172 :   if (activation_control_output_.diagnostics.mass_estimator) {
+     457           6 :     uav_mass_difference_ = activation_control_output_.diagnostics.mass_difference;
+     458           6 :     activation_mass += uav_mass_difference_;
+     459           6 :     ROS_INFO("[%s]: setting mass difference from the last control output: %.2f kg", this->name_.c_str(), uav_mass_difference_);
+     460             :   }
+     461             : 
+     462         172 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+     463             : 
+     464         172 :   if (activation_control_output_.diagnostics.disturbance_estimator) {
+     465           6 :     Ib_b_[0] = -activation_control_output_.diagnostics.disturbance_bx_b;
+     466           6 :     Ib_b_[1] = -activation_control_output_.diagnostics.disturbance_by_b;
+     467             : 
+     468           6 :     Iw_w_[0] = -activation_control_output_.diagnostics.disturbance_wx_w;
+     469           6 :     Iw_w_[1] = -activation_control_output_.diagnostics.disturbance_wy_w;
+     470             : 
+     471           6 :     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         172 :   auto throttle_last_controller = common::extractThrottle(activation_control_output_);
+     479             : 
+     480             :   // rampup check
+     481         172 :   if (_rampup_enabled_ && throttle_last_controller) {
+     482             : 
+     483          54 :     double hover_throttle = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, activation_mass * common_handlers_->g);
+     484             : 
+     485          54 :     double throttle_difference = hover_throttle - throttle_last_controller.value();
+     486             : 
+     487          54 :     if (throttle_difference > 0) {
+     488          31 :       rampup_direction_ = 1;
+     489          23 :     } else if (throttle_difference < 0) {
+     490           3 :       rampup_direction_ = -1;
+     491             :     } else {
+     492          20 :       rampup_direction_ = 0;
+     493             :     }
+     494             : 
+     495          54 :     ROS_INFO("[%s]: activating rampup with initial throttle: %.4f, target: %.4f", name_.c_str(), throttle_last_controller.value(), hover_throttle);
+     496             : 
+     497          54 :     rampup_active_     = true;
+     498          54 :     rampup_start_time_ = ros::Time::now();
+     499          54 :     rampup_last_time_  = ros::Time::now();
+     500          54 :     rampup_throttle_   = throttle_last_controller.value();
+     501             : 
+     502          54 :     rampup_duration_ = fabs(throttle_difference) / _rampup_speed_;
+     503             :   }
+     504             : 
+     505         172 :   first_iteration_ = true;
+     506         172 :   mute_gains_      = true;
+     507             : 
+     508         172 :   timer_gains_.start();
+     509             : 
+     510         172 :   ROS_INFO("[%s]: activated", this->name_.c_str());
+     511             : 
+     512         172 :   is_active_ = true;
+     513             : 
+     514         172 :   return true;
+     515             : }
+     516             : 
+     517             : //}
+     518             : 
+     519             : /* //{ deactivate() */
+     520             : 
+     521         138 : void MpcController::deactivate(void) {
+     522             : 
+     523         138 :   is_active_           = false;
+     524         138 :   first_iteration_     = false;
+     525         138 :   uav_mass_difference_ = 0;
+     526             : 
+     527         138 :   timer_gains_.stop();
+     528             : 
+     529         138 :   ROS_INFO("[%s]: deactivated", this->name_.c_str());
+     530         138 : }
+     531             : 
+     532             : //}
+     533             : 
+     534             : /* updateInactive() //{ */
+     535             : 
+     536      192965 : void MpcController::updateInactive(const mrs_msgs::UavState &uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &tracker_command) {
+     537             : 
+     538      192965 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     539             : 
+     540      192965 :   last_update_time_ = uav_state.header.stamp;
+     541             : 
+     542      192965 :   first_iteration_ = false;
+     543      192965 : }
+     544             : 
+     545             : //}
+     546             : 
+     547             : /* //{ updateWhenAcctive() */
+     548             : 
+     549       49659 : MpcController::ControlOutput MpcController::updateActive(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+     550             : 
+     551      148977 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateActive");
+     552      148977 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcController::updateActive", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     553             : 
+     554       99318 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     555             : 
+     556       49659 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     557             : 
+     558       49659 :   last_control_output_.desired_heading_rate          = {};
+     559       49659 :   last_control_output_.desired_orientation           = {};
+     560       49659 :   last_control_output_.desired_unbiased_acceleration = {};
+     561       49659 :   last_control_output_.control_output                = {};
+     562             : 
+     563       49659 :   if (!is_active_) {
+     564           0 :     return last_control_output_;
+     565             :   }
+     566             : 
+     567             :   // | -------------------- calculate the dt -------------------- |
+     568             : 
+     569             :   double dt;
+     570             : 
+     571       49659 :   if (first_iteration_) {
+     572          56 :     dt               = 0.01;
+     573          56 :     first_iteration_ = false;
+     574             :   } else {
+     575       49603 :     dt = (uav_state.header.stamp - last_update_time_).toSec();
+     576             :   }
+     577             : 
+     578       49659 :   last_update_time_ = uav_state.header.stamp;
+     579             : 
+     580       49659 :   if (fabs(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       49659 :   auto lowest_modality = common::getLowestOuput(common_handlers_->control_output_modalities);
+     590             : 
+     591       49659 :   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       49659 :   if (drs_params.preferred_output_mode == OUTPUT_ATTITUDE_RATE && common_handlers_->control_output_modalities.attitude_rate) {
+     601       44156 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: prioritizing attitude rate output", name_.c_str());
+     602       44156 :     lowest_modality = common::ATTITUDE_RATE;
+     603        5503 :   } 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        5503 :   } 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        5503 :   } 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       49659 :   switch (lowest_modality.value()) {
+     615             : 
+     616         250 :     case common::POSITION: {
+     617         250 :       positionPassthrough(uav_state, tracker_command);
+     618         250 :       break;
+     619             :     }
+     620             : 
+     621         249 :     case common::VELOCITY_HDG: {
+     622         249 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG, dt);
+     623         249 :       break;
+     624             :     }
+     625             : 
+     626         496 :     case common::VELOCITY_HDG_RATE: {
+     627         496 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG_RATE, dt);
+     628         496 :       break;
+     629             :     }
+     630             : 
+     631         539 :     case common::ACCELERATION_HDG: {
+     632         539 :       MPC(uav_state, tracker_command, dt, common::ACCELERATION_HDG);
+     633         539 :       break;
+     634             :     }
+     635             : 
+     636         497 :     case common::ACCELERATION_HDG_RATE: {
+     637         497 :       MPC(uav_state, tracker_command, dt, common::ACCELERATION_HDG_RATE);
+     638         497 :       break;
+     639             :     }
+     640             : 
+     641        1085 :     case common::ATTITUDE: {
+     642        1085 :       MPC(uav_state, tracker_command, dt, common::ATTITUDE);
+     643        1085 :       break;
+     644             :     }
+     645             : 
+     646       44156 :     case common::ATTITUDE_RATE: {
+     647       44156 :       MPC(uav_state, tracker_command, dt, common::ATTITUDE_RATE);
+     648       44156 :       break;
+     649             :     }
+     650             : 
+     651        1203 :     case common::CONTROL_GROUP: {
+     652        1203 :       MPC(uav_state, tracker_command, dt, common::CONTROL_GROUP);
+     653        1203 :       break;
+     654             :     }
+     655             : 
+     656        1184 :     case common::ACTUATORS_CMD: {
+     657        1184 :       MPC(uav_state, tracker_command, dt, common::ACTUATORS_CMD);
+     658        1184 :       break;
+     659             :     }
+     660             : 
+     661           0 :     default: {
+     662           0 :       break;
+     663             :     }
+     664             :   }
+     665             : 
+     666       49659 :   return last_control_output_;
+     667             : }
+     668             : 
+     669             : //}
+     670             : 
+     671             : /* //{ getStatus() */
+     672             : 
+     673       11296 : const mrs_msgs::ControllerStatus MpcController::getStatus() {
+     674             : 
+     675       11296 :   mrs_msgs::ControllerStatus controller_status;
+     676             : 
+     677       11296 :   controller_status.active = is_active_;
+     678             : 
+     679       11296 :   return controller_status;
+     680             : }
+     681             : 
+     682             : //}
+     683             : 
+     684             : /* switchOdometrySource() //{ */
+     685             : 
+     686           4 : void MpcController::switchOdometrySource(const mrs_msgs::UavState &new_uav_state) {
+     687             : 
+     688           4 :   ROS_INFO("[%s]: switching the odometry source", this->name_.c_str());
+     689             : 
+     690           8 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     691             : 
+     692             :   // | ----- transform world disturabances to the new frame ----- |
+     693             : 
+     694           8 :   geometry_msgs::Vector3Stamped world_integrals;
+     695             : 
+     696           4 :   world_integrals.header.stamp    = ros::Time::now();
+     697           4 :   world_integrals.header.frame_id = uav_state.header.frame_id;
+     698             : 
+     699           4 :   world_integrals.vector.x = Iw_w_[0];
+     700           4 :   world_integrals.vector.y = Iw_w_[1];
+     701           4 :   world_integrals.vector.z = 0;
+     702             : 
+     703           8 :   auto res = common_handlers_->transformer->transformSingle(world_integrals, new_uav_state.header.frame_id);
+     704             : 
+     705           4 :   if (res) {
+     706             : 
+     707           4 :     std::scoped_lock lock(mutex_integrals_);
+     708             : 
+     709           4 :     Iw_w_[0] = res.value().vector.x;
+     710           4 :     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           4 : }
+     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         638 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MpcController::setConstraints([
+     740             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &constraints) {
+     741             : 
+     742         638 :   if (!is_initialized_) {
+     743           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     744             :   }
+     745             : 
+     746         638 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     747             : 
+     748         638 :   ROS_INFO("[%s]: updating constraints", this->name_.c_str());
+     749             : 
+     750        1276 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     751         638 :   res.success = true;
+     752         638 :   res.message = "constraints updated";
+     753             : 
+     754         638 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     755             : }
+     756             : 
+     757             : //}
+     758             : 
+     759             : // --------------------------------------------------------------
+     760             : // |                         controllers                        |
+     761             : // --------------------------------------------------------------
+     762             : 
+     763             : /* Mpc() //{ */
+     764             : 
+     765       48664 : 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       97328 :   auto drs_params  = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     769       48664 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     770       48664 :   auto gains       = mrs_lib::get_mutexed(mutex_gains_, gains_);
+     771             : 
+     772             :   // | ----------------- get the current heading ---------------- |
+     773             : 
+     774       48664 :   double uav_heading = getHeadingSafely(uav_state, tracker_command);
+     775             : 
+     776             :   // | ------------------- prepare constraints ------------------ |
+     777             : 
+     778       48664 :   double max_speed_horizontal        = _max_speed_horizontal_;
+     779       48664 :   double max_speed_vertical          = _max_speed_vertical_;
+     780       48664 :   double max_acceleration_horizontal = _max_acceleration_horizontal_;
+     781       48664 :   double max_acceleration_vertical   = _max_acceleration_vertical_;
+     782       48664 :   double max_jerk                    = _max_jerk_;
+     783       48664 :   double max_u_vertical              = _max_u_vertical_;
+     784             : 
+     785       48664 :   if (tracker_command.use_full_state_prediction) {
+     786             : 
+     787       31824 :     max_speed_horizontal = 1.5 * (constraints.horizontal_speed);
+     788       31824 :     max_speed_vertical   = 1.5 * (constraints.vertical_ascending_speed < constraints.vertical_descending_speed ? constraints.vertical_ascending_speed
+     789             :                                                                                                              : constraints.vertical_descending_speed);
+     790             : 
+     791       31824 :     max_acceleration_horizontal = 1.5 * (constraints.horizontal_acceleration);
+     792       31824 :     max_acceleration_vertical =
+     793       31824 :         1.5 * (constraints.vertical_ascending_acceleration < constraints.vertical_descending_acceleration ? constraints.vertical_ascending_acceleration
+     794             :                                                                                                           : constraints.vertical_descending_acceleration);
+     795             : 
+     796       31824 :     max_jerk       = 1.5 * constraints.horizontal_jerk;
+     797       31824 :     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       48664 :   Eigen::Vector3d Rp = Eigen::Vector3d::Zero(3);
+     810       48664 :   Eigen::Vector3d Rv = Eigen::Vector3d::Zero(3);
+     811       48664 :   Eigen::Vector3d Ra = Eigen::Vector3d::Zero(3);
+     812             : 
+     813       48664 :   Rp << tracker_command.position.x, tracker_command.position.y, tracker_command.position.z;  // fill the desired position
+     814       48664 :   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       48664 :   Eigen::Vector3d Op(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+     821       48664 :   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       48664 :   Eigen::Matrix3d R = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+     825             : 
+     826             :   // Ow - UAV angular rate
+     827       48664 :   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       48664 :   Eigen::Vector3d Ep = Rp - Op;
+     836       48664 :   Eigen::Vector3d Ev = Rv - Ov;
+     837             : 
+     838             :   // | ------------------- initial conditions ------------------- |
+     839             : 
+     840       97328 :   Eigen::MatrixXd initial_x = Eigen::MatrixXd::Zero(3, 1);
+     841       97328 :   Eigen::MatrixXd initial_y = Eigen::MatrixXd::Zero(3, 1);
+     842       97328 :   Eigen::MatrixXd initial_z = Eigen::MatrixXd::Zero(3, 1);
+     843             : 
+     844             :   /* initial x //{ */
+     845             : 
+     846             :   {
+     847             :     double acceleration;
+     848             :     double velocity;
+     849       48664 :     double coef = 1.5;
+     850             : 
+     851       48664 :     if (fabs(uav_state.acceleration.linear.x) < coef * max_acceleration_horizontal) {
+     852       48664 :       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             :                          fabs(uav_state.acceleration.linear.x), coef, max_acceleration_horizontal);
+     858             :     }
+     859             : 
+     860       48664 :     if (fabs(uav_state.velocity.linear.x) < coef * max_speed_horizontal) {
+     861       48664 :       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             :                          fabs(uav_state.velocity.linear.x), coef, max_speed_horizontal);
+     867             :     }
+     868             : 
+     869       48664 :     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       48664 :     double coef = 1.5;
+     880             : 
+     881       48664 :     if (fabs(uav_state.acceleration.linear.y) < coef * max_acceleration_horizontal) {
+     882       48664 :       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             :                          fabs(uav_state.acceleration.linear.y), coef, max_acceleration_horizontal);
+     888             :     }
+     889             : 
+     890       48664 :     if (fabs(uav_state.velocity.linear.y) < coef * max_speed_horizontal) {
+     891       48664 :       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             :                          fabs(uav_state.velocity.linear.y), coef, max_speed_horizontal);
+     897             :     }
+     898             : 
+     899       48664 :     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       48664 :     double coef = 1.5;
+     910             : 
+     911       48664 :     if (fabs(uav_state.acceleration.linear.z) < coef * max_acceleration_horizontal) {
+     912       48664 :       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             :                          fabs(uav_state.acceleration.linear.z), coef, max_acceleration_horizontal);
+     918             :     }
+     919             : 
+     920       48664 :     if (fabs(uav_state.velocity.linear.z) < coef * max_speed_vertical) {
+     921       48664 :       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             :                          fabs(uav_state.velocity.linear.z), coef, max_speed_vertical);
+     927             :     }
+     928             : 
+     929       48664 :     initial_z << uav_state.pose.position.z, velocity, acceleration;
+     930             :   }
+     931             : 
+     932             :   //}
+     933             : 
+     934             :   // | ---------------------- set reference --------------------- |
+     935             : 
+     936       97328 :   Eigen::MatrixXd mpc_reference_x = Eigen::MatrixXd::Zero(_horizon_length_ * _n_states_, 1);
+     937       97328 :   Eigen::MatrixXd mpc_reference_y = Eigen::MatrixXd::Zero(_horizon_length_ * _n_states_, 1);
+     938       97328 :   Eigen::MatrixXd mpc_reference_z = Eigen::MatrixXd::Zero(_horizon_length_ * _n_states_, 1);
+     939             : 
+     940             :   // prepare the full reference vector
+     941       48664 :   if (tracker_command.use_full_state_prediction) {
+     942             : 
+     943       31824 :     mpc_reference_x(0, 0) = tracker_command.position.x;
+     944       31824 :     mpc_reference_y(0, 0) = tracker_command.position.y;
+     945       31824 :     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      827424 :     for (int i = 1; i < _horizon_length_; i++) {
+     951      795600 :       mpc_reference_x((i * _n_states_) + 0, 0) = tracker_command.full_state_prediction.position[i].x;
+     952      795600 :       mpc_reference_y((i * _n_states_) + 0, 0) = tracker_command.full_state_prediction.position[i].y;
+     953      795600 :       mpc_reference_z((i * _n_states_) + 0, 0) = tracker_command.full_state_prediction.position[i].z;
+     954             :     }
+     955             : 
+     956      827424 :     for (int i = 1; i < _horizon_length_; i++) {
+     957      795600 :       mpc_reference_x((i * _n_states_) + 1, 0) = tracker_command.full_state_prediction.velocity[i].x;
+     958      795600 :       mpc_reference_y((i * _n_states_) + 1, 0) = tracker_command.full_state_prediction.velocity[i].y;
+     959      795600 :       mpc_reference_z((i * _n_states_) + 1, 0) = tracker_command.full_state_prediction.velocity[i].z;
+     960             :     }
+     961             : 
+     962      827424 :     for (int i = 1; i < _horizon_length_; i++) {
+     963      795600 :       mpc_reference_x((i * _n_states_) + 2, 0) = tracker_command.full_state_prediction.acceleration[i].x;
+     964      795600 :       mpc_reference_y((i * _n_states_) + 2, 0) = tracker_command.full_state_prediction.acceleration[i].y;
+     965      795600 :       mpc_reference_z((i * _n_states_) + 2, 0) = tracker_command.full_state_prediction.acceleration[i].z;
+     966             :     }
+     967             : 
+     968             :   } else {
+     969             : 
+     970      454680 :     for (int i = 0; i < _horizon_length_; i++) {
+     971      437840 :       mpc_reference_x((i * _n_states_) + 0, 0) = tracker_command.position.x;
+     972      437840 :       mpc_reference_y((i * _n_states_) + 0, 0) = tracker_command.position.y;
+     973      437840 :       mpc_reference_z((i * _n_states_) + 0, 0) = tracker_command.position.z;
+     974             :     }
+     975             :   }
+     976             : 
+     977             :   // | ------------------ set the penalizations ----------------- |
+     978             : 
+     979       97328 :   std::vector<double> temp_Q_horizontal = _mat_Q_;
+     980       97328 :   std::vector<double> temp_Q_vertical   = _mat_Q_z_;
+     981             : 
+     982       97328 :   std::vector<double> temp_S_horizontal = _mat_S_;
+     983       97328 :   std::vector<double> temp_S_vertical   = _mat_S_z_;
+     984             : 
+     985       48664 :   if (!tracker_command.use_position_horizontal) {
+     986           0 :     temp_Q_horizontal[0] = 0;
+     987           0 :     temp_S_horizontal[0] = 0;
+     988             :   }
+     989             : 
+     990       48664 :   if (!tracker_command.use_velocity_horizontal) {
+     991           0 :     temp_Q_horizontal[1] = 0;
+     992           0 :     temp_S_horizontal[1] = 0;
+     993             :   }
+     994             : 
+     995       48664 :   if (!tracker_command.use_position_vertical) {
+     996           0 :     temp_Q_vertical[0] = 0;
+     997           0 :     temp_S_vertical[0] = 0;
+     998             :   }
+     999             : 
+    1000       48664 :   if (!tracker_command.use_velocity_vertical) {
+    1001           0 :     temp_Q_vertical[1] = 0;
+    1002           0 :     temp_S_vertical[1] = 0;
+    1003             :   }
+    1004             : 
+    1005             :   // | ------------------------ optimize ------------------------ |
+    1006             : 
+    1007       48664 :   mpc_solver_x_->setQ(temp_Q_horizontal);
+    1008       48664 :   mpc_solver_x_->setS(temp_S_horizontal);
+    1009       48664 :   mpc_solver_x_->setParams();
+    1010       48664 :   mpc_solver_x_->setLastInput(mpc_solver_x_u_);
+    1011       48664 :   mpc_solver_x_->loadReference(mpc_reference_x);
+    1012       48664 :   mpc_solver_x_->setLimits(max_speed_horizontal, 999, max_acceleration_horizontal, max_jerk, _dt1_, _dt2_);
+    1013       48664 :   mpc_solver_x_->setInitialState(initial_x);
+    1014       48664 :   [[maybe_unused]] int iters_x = mpc_solver_x_->solveMPC();
+    1015       48664 :   mpc_solver_x_u_              = mpc_solver_x_->getFirstControlInput();
+    1016             : 
+    1017       48664 :   mpc_solver_y_->setQ(temp_Q_horizontal);
+    1018       48664 :   mpc_solver_y_->setS(temp_S_horizontal);
+    1019       48664 :   mpc_solver_y_->setParams();
+    1020       48664 :   mpc_solver_y_->setLastInput(mpc_solver_y_u_);
+    1021       48664 :   mpc_solver_y_->loadReference(mpc_reference_y);
+    1022       48664 :   mpc_solver_y_->setLimits(max_speed_horizontal, 999, max_acceleration_horizontal, max_jerk, _dt1_, _dt2_);
+    1023       48664 :   mpc_solver_y_->setInitialState(initial_y);
+    1024       48664 :   [[maybe_unused]] int iters_y = mpc_solver_y_->solveMPC();
+    1025       48664 :   mpc_solver_y_u_              = mpc_solver_y_->getFirstControlInput();
+    1026             : 
+    1027       48664 :   mpc_solver_z_->setQ(temp_Q_vertical);
+    1028       48664 :   mpc_solver_z_->setS(temp_S_vertical);
+    1029       48664 :   mpc_solver_z_->setParams();
+    1030       48664 :   mpc_solver_z_->setLastInput(mpc_solver_z_u_);
+    1031       48664 :   mpc_solver_z_->loadReference(mpc_reference_z);
+    1032       48664 :   mpc_solver_z_->setLimits(max_speed_vertical, max_acceleration_vertical, max_u_vertical, 999.0, _dt1_, _dt2_);
+    1033       48664 :   mpc_solver_z_->setInitialState(initial_z);
+    1034       48664 :   [[maybe_unused]] int iters_z = mpc_solver_z_->solveMPC();
+    1035       48664 :   mpc_solver_z_u_              = mpc_solver_z_->getFirstControlInput();
+    1036             : 
+    1037             :   // | ----------- disable lateral feedback if needed ----------- |
+    1038             : 
+    1039       48664 :   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       48664 :   mute_gains_by_tracker_ = tracker_command.disable_position_gains;
+    1047             : 
+    1048       48664 :   Eigen::Array3d Kw(0, 0, 0);
+    1049       48664 :   Eigen::Array3d Kq;
+    1050             : 
+    1051             :   {
+    1052       48664 :     std::scoped_lock lock(mutex_gains_);
+    1053             : 
+    1054       48664 :     Kq << gains.kq_roll_pitch, gains.kq_roll_pitch, gains.kq_yaw;
+    1055             : 
+    1056       48664 :     Kw[0] = gains.kw_rp;
+    1057       48664 :     Kw[1] = gains.kw_rp;
+    1058       48664 :     Kw[2] = gains.kw_y;
+    1059             :   }
+    1060             : 
+    1061             :   // | ---------- desired orientation matrix and force ---------- |
+    1062             : 
+    1063             :   // get body integral in the world frame
+    1064             : 
+    1065       48664 :   Eigen::Vector2d Ib_w = Eigen::Vector2d(0, 0);
+    1066             : 
+    1067             :   {
+    1068             : 
+    1069       97328 :     geometry_msgs::Vector3Stamped Ib_b_stamped;
+    1070             : 
+    1071       48664 :     Ib_b_stamped.header.stamp    = ros::Time::now();
+    1072       48664 :     Ib_b_stamped.header.frame_id = "fcu_untilted";
+    1073       48664 :     Ib_b_stamped.vector.x        = Ib_b_(0);
+    1074       48664 :     Ib_b_stamped.vector.y        = Ib_b_(1);
+    1075       48664 :     Ib_b_stamped.vector.z        = 0;
+    1076             : 
+    1077       97328 :     auto res = common_handlers_->transformer->transformSingle(Ib_b_stamped, uav_state_.header.frame_id);
+    1078             : 
+    1079       48664 :     if (res) {
+    1080       48664 :       Ib_w[0] = res.value().vector.x;
+    1081       48664 :       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       48664 :   if (tracker_command.use_acceleration && !tracker_command.use_full_state_prediction) {
+    1090        1662 :     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       47002 :     Ra << mpc_solver_x_u_, mpc_solver_y_u_, mpc_solver_z_u_;
+    1093             :   }
+    1094             : 
+    1095       48664 :   double total_mass = _uav_mass_ + uav_mass_difference_;
+    1096             : 
+    1097       48664 :   Eigen::Vector3d feed_forward = total_mass * (Eigen::Vector3d(0, 0, common_handlers_->g) + Ra);
+    1098             : 
+    1099       48664 :   Eigen::Vector3d integral_feedback;
+    1100             :   {
+    1101       48664 :     std::scoped_lock lock(mutex_integrals_);
+    1102             : 
+    1103       48664 :     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       97328 :     std::scoped_lock lock(mutex_gains_, mutex_integrals_);
+    1118             : 
+    1119       48664 :     Eigen::Vector3d integration_switch(1, 1, 0);
+    1120             : 
+    1121             :     // integrate the world error
+    1122             : 
+    1123             :     // antiwindup
+    1124       48664 :     double temp_gain = gains.kiwxy;
+    1125       48664 :     if (!tracker_command.disable_antiwindups) {
+    1126       40453 :       if (rampup_active_ || sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2)) > 0.3) {
+    1127       17497 :         temp_gain = 0;
+    1128       17497 :         ROS_DEBUG_THROTTLE(1.0, "[%s]: anti-windup for world integral kicks in", this->name_.c_str());
+    1129             :       }
+    1130             :     }
+    1131             : 
+    1132       48664 :     if (integral_terms_enabled_) {
+    1133       48664 :       if (tracker_command.use_position_horizontal) {
+    1134       48664 :         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       48664 :     bool world_integral_saturated = false;
+    1142       48664 :     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       48664 :     } else if (Iw_w_[0] > gains.kiwxy_lim) {
+    1146           0 :       Iw_w_[0]                 = gains.kiwxy_lim;
+    1147           0 :       world_integral_saturated = true;
+    1148       48664 :     } 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       48664 :     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       48664 :     world_integral_saturated = false;
+    1159       48664 :     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       48664 :     } else if (Iw_w_[1] > gains.kiwxy_lim) {
+    1163           0 :       Iw_w_[1]                 = gains.kiwxy_lim;
+    1164           0 :       world_integral_saturated = true;
+    1165       48664 :     } 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       48664 :     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       97328 :     std::scoped_lock lock(mutex_gains_);
+    1181             : 
+    1182       48664 :     Eigen::Vector2d Ep_fcu_untilted = Eigen::Vector2d(0, 0);  // position error in the untilted frame of the UAV
+    1183       48664 :     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       97328 :       geometry_msgs::Vector3Stamped Ep_stamped;
+    1189             : 
+    1190       48664 :       Ep_stamped.header.stamp    = ros::Time::now();
+    1191       48664 :       Ep_stamped.header.frame_id = uav_state_.header.frame_id;
+    1192       48664 :       Ep_stamped.vector.x        = Ep(0);
+    1193       48664 :       Ep_stamped.vector.y        = Ep(1);
+    1194       48664 :       Ep_stamped.vector.z        = Ep(2);
+    1195             : 
+    1196      145992 :       auto res = common_handlers_->transformer->transformSingle(Ep_stamped, "fcu_untilted");
+    1197             : 
+    1198       48664 :       if (res) {
+    1199       48664 :         Ep_fcu_untilted[0] = res.value().vector.x;
+    1200       48664 :         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       97328 :       geometry_msgs::Vector3Stamped Ev_stamped;
+    1209             : 
+    1210       48664 :       Ev_stamped.header.stamp    = ros::Time::now();
+    1211       48664 :       Ev_stamped.header.frame_id = uav_state_.header.frame_id;
+    1212       48664 :       Ev_stamped.vector.x        = Ev(0);
+    1213       48664 :       Ev_stamped.vector.y        = Ev(1);
+    1214       48664 :       Ev_stamped.vector.z        = Ev(2);
+    1215             : 
+    1216      145992 :       auto res = common_handlers_->transformer->transformSingle(Ev_stamped, "fcu_untilted");
+    1217             : 
+    1218       48664 :       if (res) {
+    1219       48664 :         Ev_fcu_untilted[0] = res.value().vector.x;
+    1220       48664 :         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       48664 :     double temp_gain = gains.kibxy;
+    1230       48664 :     if (!tracker_command.disable_antiwindups) {
+    1231       40453 :       if (rampup_active_ || sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2)) > 0.3) {
+    1232       17497 :         temp_gain = 0;
+    1233       17497 :         ROS_DEBUG_THROTTLE(1.0, "[%s]: anti-windup for body integral kicks in", this->name_.c_str());
+    1234             :       }
+    1235             :     }
+    1236             : 
+    1237       48664 :     if (integral_terms_enabled_) {
+    1238       48664 :       if (tracker_command.use_position_horizontal) {
+    1239       48664 :         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       48664 :     bool body_integral_saturated = false;
+    1247       48664 :     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       48664 :     } else if (Ib_b_[0] > gains.kibxy_lim) {
+    1251           0 :       Ib_b_[0]                = gains.kibxy_lim;
+    1252           0 :       body_integral_saturated = true;
+    1253       48664 :     } 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       48664 :     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       48664 :     body_integral_saturated = false;
+    1264       48664 :     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       48664 :     } else if (Ib_b_[1] > gains.kibxy_lim) {
+    1268           0 :       Ib_b_[1]                = gains.kibxy_lim;
+    1269           0 :       body_integral_saturated = true;
+    1270       48664 :     } 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       48664 :     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       48664 :   Eigen::Vector3d des_acc = integral_feedback / total_mass + Ra;
+    1283             : 
+    1284       48664 :   if (output_modality == common::ACCELERATION_HDG || output_modality == common::ACCELERATION_HDG_RATE) {
+    1285             : 
+    1286        1036 :     if (output_modality == common::ACCELERATION_HDG) {
+    1287             : 
+    1288        1078 :       mrs_msgs::HwApiAccelerationHdgCmd cmd;
+    1289             : 
+    1290         539 :       cmd.acceleration.x = des_acc[0];
+    1291         539 :       cmd.acceleration.y = des_acc[1];
+    1292         539 :       cmd.acceleration.z = des_acc[2];
+    1293             : 
+    1294         539 :       cmd.heading = tracker_command.heading;
+    1295             : 
+    1296         539 :       last_control_output_.control_output = cmd;
+    1297             : 
+    1298             :     } else {
+    1299             : 
+    1300         497 :       double des_hdg_ff = 0;
+    1301             : 
+    1302         497 :       if (tracker_command.use_heading_rate) {
+    1303         497 :         des_hdg_ff = tracker_command.heading_rate;
+    1304             :       }
+    1305             : 
+    1306         994 :       mrs_msgs::HwApiAccelerationHdgRateCmd cmd;
+    1307             : 
+    1308         497 :       cmd.acceleration.x = des_acc[0];
+    1309         497 :       cmd.acceleration.y = des_acc[1];
+    1310         497 :       cmd.acceleration.z = des_acc[2];
+    1311             : 
+    1312         497 :       position_pid_heading_.setSaturation(constraints.heading_speed);
+    1313             : 
+    1314         497 :       double hdg_err = mrs_lib::geometry::sradians::diff(tracker_command.heading, uav_heading);
+    1315             : 
+    1316         497 :       double des_hdg_rate = position_pid_heading_.update(hdg_err, dt) + des_hdg_ff;
+    1317             : 
+    1318         497 :       cmd.heading_rate = des_hdg_rate;
+    1319             : 
+    1320         497 :       last_control_output_.desired_heading_rate = des_hdg_rate;
+    1321             : 
+    1322         497 :       last_control_output_.control_output = cmd;
+    1323             :     }
+    1324             : 
+    1325             :     // | -------------- unbiased desired acceleration ------------- |
+    1326             : 
+    1327        1036 :     Eigen::Vector3d unbiased_des_acc(0, 0, 0);
+    1328             : 
+    1329             :     {
+    1330             : 
+    1331        2072 :       geometry_msgs::Vector3Stamped world_accel;
+    1332             : 
+    1333        1036 :       world_accel.header.stamp    = ros::Time::now();
+    1334        1036 :       world_accel.header.frame_id = uav_state.header.frame_id;
+    1335        1036 :       world_accel.vector.x        = Ra[0];
+    1336        1036 :       world_accel.vector.y        = Ra[1];
+    1337        1036 :       world_accel.vector.z        = Ra[2];
+    1338             : 
+    1339        3108 :       auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1340             : 
+    1341        1036 :       if (res) {
+    1342        1036 :         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        1036 :     last_control_output_.desired_unbiased_acceleration = unbiased_des_acc;
+    1348             : 
+    1349             :     // | ----------------- fill in the diagnostics ---------------- |
+    1350             : 
+    1351        1036 :     last_control_output_.diagnostics.ramping_up = false;
+    1352             : 
+    1353        1036 :     last_control_output_.diagnostics.mass_estimator  = false;
+    1354        1036 :     last_control_output_.diagnostics.mass_difference = 0;
+    1355        1036 :     last_control_output_.diagnostics.total_mass      = total_mass;
+    1356             : 
+    1357        1036 :     last_control_output_.diagnostics.disturbance_estimator = true;
+    1358             : 
+    1359        1036 :     last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_[0];
+    1360        1036 :     last_control_output_.diagnostics.disturbance_by_b = -Ib_b_[1];
+    1361             : 
+    1362        1036 :     last_control_output_.diagnostics.disturbance_bx_w = -Ib_w[0];
+    1363        1036 :     last_control_output_.diagnostics.disturbance_by_w = -Ib_w[1];
+    1364             : 
+    1365        1036 :     last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_[0];
+    1366        1036 :     last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_[1];
+    1367             : 
+    1368        1036 :     last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1369             : 
+    1370        1036 :     last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * max_speed_horizontal;
+    1371        1036 :     last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * max_acceleration_horizontal;
+    1372             : 
+    1373        1036 :     last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * max_speed_vertical;
+    1374        1036 :     last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * max_acceleration_vertical;
+    1375             : 
+    1376        1036 :     last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * max_speed_vertical;
+    1377        1036 :     last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * max_acceleration_vertical;
+    1378             : 
+    1379        1036 :     last_control_output_.diagnostics.controller = name_;
+    1380             : 
+    1381        1036 :     return;
+    1382             :   }
+    1383             : 
+    1384             :   /* mass estimatior //{ */
+    1385             : 
+    1386             :   // --------------------------------------------------------------
+    1387             :   // |                integrate the mass difference               |
+    1388             :   // --------------------------------------------------------------
+    1389             : 
+    1390             :   {
+    1391       95256 :     std::scoped_lock lock(mutex_gains_);
+    1392             : 
+    1393             :     // antiwindup
+    1394       47628 :     double temp_gain = gains.km;
+    1395       94302 :     if (rampup_active_ ||
+    1396       46674 :         (fabs(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       13168 :       temp_gain = 0;
+    1398       13168 :       ROS_DEBUG_THROTTLE(1.0, "[%s]: anti-windup for the mass kicks in", this->name_.c_str());
+    1399             :     }
+    1400             : 
+    1401       47628 :     if (tracker_command.use_position_vertical) {
+    1402       47628 :       uav_mass_difference_ += temp_gain * Ep[2] * dt;
+    1403             :     }
+    1404             : 
+    1405             :     // saturate the mass estimator
+    1406       47628 :     bool uav_mass_saturated = false;
+    1407       47628 :     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       47628 :     } else if (uav_mass_difference_ > gains.km_lim) {
+    1411           0 :       uav_mass_difference_ = gains.km_lim;
+    1412           0 :       uav_mass_saturated   = true;
+    1413       47628 :     } 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       47628 :     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       47628 :   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       47628 :   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       47628 :   double tilt_safety_limit = _tilt_angle_failsafe_enabled_ ? _tilt_angle_failsafe_ : std::numeric_limits<double>::max();
+    1441             : 
+    1442       47628 :   auto f_normed_sanitized = common::sanitizeDesiredForce(f.normalized(), tilt_safety_limit, constraints.tilt, "MpcController");
+    1443             : 
+    1444       47628 :   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       47628 :   Eigen::Vector3d f_normed = f_normed_sanitized.value();
+    1458             : 
+    1459             :   // --------------------------------------------------------------
+    1460             :   // |               desired orientation + throttle               |
+    1461             :   // --------------------------------------------------------------
+    1462             : 
+    1463             :   // | ------------------- desired orientation ------------------ |
+    1464             : 
+    1465       47628 :   Eigen::Matrix3d Rd;
+    1466             : 
+    1467       47628 :   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       47628 :     Eigen::Vector3d bxd;  // desired heading vector
+    1484             : 
+    1485       47628 :     if (tracker_command.use_heading) {
+    1486       47628 :       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       47628 :     Rd = common::so3transform(f_normed, bxd, false);
+    1493             :   }
+    1494             : 
+    1495             :   // | -------------------- desired throttle -------------------- |
+    1496             : 
+    1497       47628 :   double desired_thrust_force = f.dot(R.col(2));
+    1498       47628 :   double throttle             = 0;
+    1499             : 
+    1500       47628 :   if (rampup_active_) {
+    1501             : 
+    1502             :     // deactivate the rampup when the times up
+    1503         954 :     if (fabs((ros::Time::now() - rampup_start_time_).toSec()) >= rampup_duration_) {
+    1504             : 
+    1505          49 :       rampup_active_ = false;
+    1506             : 
+    1507          49 :       ROS_INFO("[%s]: rampup finished", name_.c_str());
+    1508             : 
+    1509             :     } else {
+    1510             : 
+    1511         905 :       double rampup_dt = (ros::Time::now() - rampup_last_time_).toSec();
+    1512             : 
+    1513         905 :       rampup_throttle_ += double(rampup_direction_) * _rampup_speed_ * rampup_dt;
+    1514             : 
+    1515         905 :       rampup_last_time_ = ros::Time::now();
+    1516             : 
+    1517         905 :       throttle = rampup_throttle_;
+    1518             : 
+    1519         905 :       ROS_INFO_THROTTLE(0.1, "[%s]: ramping up throttle, %.4f", name_.c_str(), throttle);
+    1520             :     }
+    1521             : 
+    1522             :   } else {
+    1523             : 
+    1524       46674 :     if (desired_thrust_force >= 0) {
+    1525       46674 :       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       47628 :   bool throttle_saturated = false;
+    1534             : 
+    1535       47628 :   if (!std::isfinite(throttle)) {
+    1536             : 
+    1537           0 :     ROS_ERROR("[%s]: NaN detected in variable 'throttle'!!!", name_.c_str());
+    1538           0 :     return;
+    1539             : 
+    1540       47628 :   } else if (throttle > _throttle_saturation_) {
+    1541           0 :     throttle = _throttle_saturation_;
+    1542           0 :     ROS_WARN_THROTTLE(0.1, "[%s]: saturating throttle to %.2f", name_.c_str(), _throttle_saturation_);
+    1543       47628 :   } 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       47628 :   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       47628 :   double desired_x_accel = 0;
+    1569       47628 :   double desired_y_accel = 0;
+    1570       47628 :   double desired_z_accel = 0;
+    1571             : 
+    1572             :   {
+    1573       47628 :     Eigen::Vector3d thrust_vector = desired_thrust_force * Rd.col(2);
+    1574             : 
+    1575       47628 :     double world_accel_x = (thrust_vector[0] / total_mass) - (Iw_w_[0] / total_mass) - (Ib_w[0] / total_mass);
+    1576       47628 :     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       47628 :     double world_accel_z = tracker_command.acceleration.z;
+    1580             : 
+    1581       95256 :     geometry_msgs::Vector3Stamped world_accel;
+    1582             : 
+    1583       47628 :     world_accel.header.stamp    = ros::Time::now();
+    1584       47628 :     world_accel.header.frame_id = uav_state.header.frame_id;
+    1585       47628 :     world_accel.vector.x        = world_accel_x;
+    1586       47628 :     world_accel.vector.y        = world_accel_y;
+    1587       47628 :     world_accel.vector.z        = world_accel_z;
+    1588             : 
+    1589      142884 :     auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1590             : 
+    1591       47628 :     if (res) {
+    1592             : 
+    1593       47628 :       desired_x_accel = res.value().vector.x;
+    1594       47628 :       desired_y_accel = res.value().vector.y;
+    1595       47628 :       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       47628 :   last_control_output_.desired_orientation = mrs_lib::AttitudeConverter(Rd);
+    1603             : 
+    1604             :   // fill the unbiased desired accelerations
+    1605       47628 :   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       47628 :   last_control_output_.diagnostics.ramping_up = rampup_active_;
+    1610             : 
+    1611       47628 :   last_control_output_.diagnostics.mass_estimator  = true;
+    1612       47628 :   last_control_output_.diagnostics.mass_difference = uav_mass_difference_;
+    1613       47628 :   last_control_output_.diagnostics.total_mass      = total_mass;
+    1614             : 
+    1615       47628 :   last_control_output_.diagnostics.disturbance_estimator = true;
+    1616             : 
+    1617       47628 :   last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_[0];
+    1618       47628 :   last_control_output_.diagnostics.disturbance_by_b = -Ib_b_[1];
+    1619             : 
+    1620       47628 :   last_control_output_.diagnostics.disturbance_bx_w = -Ib_w[0];
+    1621       47628 :   last_control_output_.diagnostics.disturbance_by_w = -Ib_w[1];
+    1622             : 
+    1623       47628 :   last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_[0];
+    1624       47628 :   last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_[1];
+    1625             : 
+    1626       47628 :   last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1627             : 
+    1628       47628 :   last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * _max_speed_horizontal_;
+    1629       47628 :   last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * _max_acceleration_horizontal_;
+    1630             : 
+    1631       47628 :   last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1632       47628 :   last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1633             : 
+    1634       47628 :   last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1635       47628 :   last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1636             : 
+    1637       47628 :   last_control_output_.diagnostics.controller = name_;
+    1638             : 
+    1639             :   // | ------------ construct the attitude reference ------------ |
+    1640             : 
+    1641       47628 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+    1642             : 
+    1643       47628 :   attitude_cmd.stamp       = ros::Time::now();
+    1644       47628 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(Rd);
+    1645       47628 :   attitude_cmd.throttle    = throttle;
+    1646             : 
+    1647       47628 :   if (output_modality == common::ATTITUDE) {
+    1648             : 
+    1649        1085 :     last_control_output_.control_output = attitude_cmd;
+    1650             : 
+    1651        1085 :     return;
+    1652             :   }
+    1653             : 
+    1654             :   // --------------------------------------------------------------
+    1655             :   // |                      attitude control                      |
+    1656             :   // --------------------------------------------------------------
+    1657             : 
+    1658       46543 :   Eigen::Vector3d rate_feedforward = Eigen::Vector3d::Zero(3);
+    1659             : 
+    1660       46543 :   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       46543 :   } else if (tracker_command.use_heading_rate) {
+    1665             : 
+    1666             :     // to fill in the feed forward yaw rate
+    1667       46539 :     double desired_yaw_rate = 0;
+    1668             : 
+    1669             :     try {
+    1670       46539 :       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       46539 :     rate_feedforward << 0, 0, desired_yaw_rate;
+    1677             :   }
+    1678             : 
+    1679             :   // | ------------ jerk feedforward -> angular rate ------------ |
+    1680             : 
+    1681       46543 :   Eigen::Vector3d jerk_feedforward = Eigen::Vector3d(0, 0, 0);
+    1682             : 
+    1683       46543 :   if (tracker_command.use_jerk && drs_params.jerk_feedforward) {
+    1684             : 
+    1685       29706 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: using jerk feedforward", name_.c_str());
+    1686             : 
+    1687       29706 :     Eigen::Matrix3d I;
+    1688       29706 :     I << 0, 1, 0, -1, 0, 0, 0, 0, 0;
+    1689       29706 :     Eigen::Vector3d desired_jerk = Eigen::Vector3d(tracker_command.jerk.x, tracker_command.jerk.y, tracker_command.jerk.z);
+    1690       29706 :     jerk_feedforward             = (I.transpose() * Rd.transpose() * desired_jerk) / (desired_thrust_force / total_mass);
+    1691             :   }
+    1692             : 
+    1693             :   // | --------------- run the attitude controller -------------- |
+    1694             : 
+    1695       46543 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+    1696             : 
+    1697       46543 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, jerk_feedforward + rate_feedforward, attitude_rate_saturation, Kq, false);
+    1698             : 
+    1699       46543 :   if (!attitude_rate_command) {
+    1700           0 :     return;
+    1701             :   }
+    1702             : 
+    1703             :   // | --------- fill in the already known attitude rate -------- |
+    1704             : 
+    1705             :   {
+    1706             :     try {
+    1707       46543 :       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       46543 :   if (output_modality == common::ATTITUDE_RATE) {
+    1716             : 
+    1717       44156 :     last_control_output_.control_output = attitude_rate_command;
+    1718             : 
+    1719       44156 :     return;
+    1720             :   }
+    1721             : 
+    1722             :   // --------------------------------------------------------------
+    1723             :   // |                    Attitude rate control                   |
+    1724             :   // --------------------------------------------------------------
+    1725             : 
+    1726        2387 :   Kw = common_handlers_->detailed_model_params->inertia.diagonal().array() * Kw;
+    1727             : 
+    1728        2387 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+    1729             : 
+    1730        2387 :   if (!control_group_command) {
+    1731           0 :     return;
+    1732             :   }
+    1733             : 
+    1734        2387 :   if (output_modality == common::CONTROL_GROUP) {
+    1735             : 
+    1736        1203 :     last_control_output_.control_output = control_group_command;
+    1737             : 
+    1738        1203 :     return;
+    1739             :   }
+    1740             : 
+    1741             :   // --------------------------------------------------------------
+    1742             :   // |                        output mixer                        |
+    1743             :   // --------------------------------------------------------------
+    1744             : 
+    1745        2368 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+    1746             : 
+    1747        1184 :   last_control_output_.control_output = actuator_cmd;
+    1748             : 
+    1749        1184 :   return;
+    1750             : }
+    1751             : 
+    1752             : //}
+    1753             : 
+    1754             : /* positionPassthrough() //{ */
+    1755             : 
+    1756         250 : void MpcController::positionPassthrough(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+    1757             : 
+    1758         250 :   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         500 :   mrs_msgs::HwApiPositionCmd cmd;
+    1764             : 
+    1765         250 :   cmd.header.frame_id = uav_state.header.frame_id;
+    1766         250 :   cmd.header.stamp    = ros::Time::now();
+    1767             : 
+    1768         250 :   cmd.position = tracker_command.position;
+    1769         250 :   cmd.heading  = tracker_command.heading;
+    1770             : 
+    1771         250 :   last_control_output_.control_output = cmd;
+    1772             : 
+    1773             :   // fill the unbiased desired accelerations
+    1774         250 :   last_control_output_.desired_unbiased_acceleration = {};
+    1775         250 :   last_control_output_.desired_orientation           = {};
+    1776         250 :   last_control_output_.desired_heading_rate          = {};
+    1777             : 
+    1778             :   // | ----------------- fill in the diagnostics ---------------- |
+    1779             : 
+    1780         250 :   last_control_output_.diagnostics.ramping_up = false;
+    1781             : 
+    1782         250 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1783         250 :   last_control_output_.diagnostics.mass_difference = 0;
+    1784             : 
+    1785         250 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1786             : 
+    1787         250 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1788         250 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1789             : 
+    1790         250 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1791         250 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1792             : 
+    1793         250 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1794         250 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1795             : 
+    1796         250 :   last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1797             : 
+    1798         250 :   last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * _max_speed_horizontal_;
+    1799         250 :   last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * _max_acceleration_horizontal_;
+    1800             : 
+    1801         250 :   last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1802         250 :   last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1803             : 
+    1804         250 :   last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1805         250 :   last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1806             : 
+    1807         250 :   last_control_output_.diagnostics.controller = name_;
+    1808             : }
+    1809             : 
+    1810             : //}
+    1811             : 
+    1812             : /* PIDVelocityOutput() //{ */
+    1813             : 
+    1814         745 : 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         745 :   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         745 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1823             : 
+    1824         745 :   Eigen::Vector3d pos_ref = Eigen::Vector3d(tracker_command.position.x, tracker_command.position.y, tracker_command.position.z);
+    1825         745 :   Eigen::Vector3d pos     = Eigen::Vector3d(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+    1826             : 
+    1827         745 :   double hdg_ref = tracker_command.heading;
+    1828         745 :   double hdg     = getHeadingSafely(uav_state, tracker_command);
+    1829             : 
+    1830             :   // | ------------------ velocity feedforward ------------------ |
+    1831             : 
+    1832         745 :   Eigen::Vector3d vel_ff(0, 0, 0);
+    1833             : 
+    1834         745 :   if (tracker_command.use_velocity_horizontal && tracker_command.use_velocity_vertical) {
+    1835         745 :     vel_ff = Eigen::Vector3d(tracker_command.velocity.x, tracker_command.velocity.y, tracker_command.velocity.z);
+    1836             :   }
+    1837             : 
+    1838             :   // | --------------------- control errors --------------------- |
+    1839             : 
+    1840         745 :   Eigen::Vector3d Ep = pos_ref - pos;
+    1841             : 
+    1842             :   // | --------------------------- pid -------------------------- |
+    1843             : 
+    1844         745 :   position_pid_x_.setSaturation(constraints.horizontal_speed);
+    1845         745 :   position_pid_y_.setSaturation(constraints.horizontal_speed);
+    1846         745 :   position_pid_z_.setSaturation(std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed));
+    1847             : 
+    1848         745 :   double des_vel_x = position_pid_x_.update(Ep[0], dt);
+    1849         745 :   double des_vel_y = position_pid_y_.update(Ep[1], dt);
+    1850         745 :   double des_vel_z = position_pid_z_.update(Ep[2], dt);
+    1851             : 
+    1852             :   // | -------------------- position feedback ------------------- |
+    1853             : 
+    1854         745 :   Eigen::Vector3d des_vel = Eigen::Vector3d(des_vel_x, des_vel_y, des_vel_z) + vel_ff;
+    1855             : 
+    1856         745 :   if (control_output == common::VELOCITY_HDG) {
+    1857             : 
+    1858             :     // | --------------------- fill the output -------------------- |
+    1859             : 
+    1860         498 :     mrs_msgs::HwApiVelocityHdgCmd cmd;
+    1861             : 
+    1862         249 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1863         249 :     cmd.header.stamp    = ros::Time::now();
+    1864             : 
+    1865         249 :     cmd.velocity.x = des_vel[0];
+    1866         249 :     cmd.velocity.y = des_vel[1];
+    1867         249 :     cmd.velocity.z = des_vel[2];
+    1868             : 
+    1869         249 :     cmd.heading = tracker_command.heading;
+    1870             : 
+    1871         249 :     last_control_output_.control_output = cmd;
+    1872             : 
+    1873         496 :   } else if (control_output == common::VELOCITY_HDG_RATE) {
+    1874             : 
+    1875         496 :     position_pid_heading_.setSaturation(constraints.heading_speed);
+    1876             : 
+    1877         496 :     double hdg_err = mrs_lib::geometry::sradians::diff(hdg_ref, hdg);
+    1878             : 
+    1879         496 :     double des_hdg_rate = position_pid_heading_.update(hdg_err, dt);
+    1880             : 
+    1881             :     // | --------------------------- ff --------------------------- |
+    1882             : 
+    1883         496 :     double des_hdg_ff = 0;
+    1884             : 
+    1885         496 :     if (tracker_command.use_heading_rate) {
+    1886         496 :       des_hdg_ff = tracker_command.heading_rate;
+    1887             :     }
+    1888             : 
+    1889             :     // | --------------------- fill the output -------------------- |
+    1890             : 
+    1891         992 :     mrs_msgs::HwApiVelocityHdgRateCmd cmd;
+    1892             : 
+    1893         496 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1894         496 :     cmd.header.stamp    = ros::Time::now();
+    1895             : 
+    1896         496 :     cmd.velocity.x = des_vel[0];
+    1897         496 :     cmd.velocity.y = des_vel[1];
+    1898         496 :     cmd.velocity.z = des_vel[2];
+    1899             : 
+    1900         496 :     cmd.heading_rate = des_hdg_rate + des_hdg_ff;
+    1901             : 
+    1902         496 :     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         745 :   last_control_output_.desired_unbiased_acceleration = {};
+    1911         745 :   last_control_output_.desired_orientation           = {};
+    1912         745 :   last_control_output_.desired_heading_rate          = {};
+    1913             : 
+    1914             :   // | ----------------- fill in the diagnostics ---------------- |
+    1915             : 
+    1916         745 :   last_control_output_.diagnostics.ramping_up = false;
+    1917             : 
+    1918         745 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1919         745 :   last_control_output_.diagnostics.mass_difference = 0;
+    1920             : 
+    1921         745 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1922             : 
+    1923         745 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1924         745 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1925             : 
+    1926         745 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1927         745 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1928             : 
+    1929         745 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1930         745 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1931             : 
+    1932         745 :   last_control_output_.diagnostics.controller_enforcing_constraints = !tracker_command.use_full_state_prediction;
+    1933             : 
+    1934         745 :   last_control_output_.diagnostics.horizontal_speed_constraint = 0.5 * _max_speed_horizontal_;
+    1935         745 :   last_control_output_.diagnostics.horizontal_acc_constraint   = 0.5 * _max_acceleration_horizontal_;
+    1936             : 
+    1937         745 :   last_control_output_.diagnostics.vertical_asc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1938         745 :   last_control_output_.diagnostics.vertical_asc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1939             : 
+    1940         745 :   last_control_output_.diagnostics.vertical_desc_speed_constraint = 0.5 * _max_speed_vertical_;
+    1941         745 :   last_control_output_.diagnostics.vertical_desc_acc_constraint   = 0.5 * _max_acceleration_vertical_;
+    1942             : 
+    1943         745 :   last_control_output_.diagnostics.controller = name_;
+    1944             : }
+    1945             : 
+    1946             : //}
+    1947             : 
+    1948             : // --------------------------------------------------------------
+    1949             : // |                          callbacks                         |
+    1950             : // --------------------------------------------------------------
+    1951             : 
+    1952             : /* //{ callbackDrs() */
+    1953             : 
+    1954         164 : void MpcController::callbackDrs(mrs_uav_controllers::mpc_controllerConfig &config, [[maybe_unused]] uint32_t level) {
+    1955             : 
+    1956         164 :   mrs_lib::set_mutexed(mutex_drs_params_, config, drs_params_);
+    1957             : 
+    1958         164 :   ROS_INFO("[%s]: DRS updated gains", this->name_.c_str());
+    1959         164 : }
+    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       12536 : void MpcController::timerGains(const ros::TimerEvent &event) {
+    1993             : 
+    1994       37608 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerGains", _gain_filtering_rate_, 1.0, event);
+    1995             :   mrs_lib::ScopeTimer timer =
+    1996       37608 :       mrs_lib::ScopeTimer("MpcController::timerGains", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1997             : 
+    1998       25072 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+    1999       12536 :   auto gains      = mrs_lib::get_mutexed(mutex_gains_, gains_);
+    2000             : 
+    2001             :   // When muting the gains, we want to bypass the filter,
+    2002             :   // so it happens immediately.
+    2003       12536 :   bool   bypass_filter = (mute_gains_ || mute_gains_by_tracker_);
+    2004       12536 :   double gain_coeff    = (mute_gains_ || mute_gains_by_tracker_) ? _gain_mute_coefficient_ : 1.0;
+    2005             : 
+    2006       12536 :   mute_gains_ = false;
+    2007             : 
+    2008       12536 :   const double dt = (event.current_real - event.last_real).toSec();
+    2009             : 
+    2010       12536 :   bool updated = false;
+    2011             : 
+    2012       12536 :   gains.kq_roll_pitch = calculateGainChange(dt, gains.kq_roll_pitch, drs_params.kq_roll_pitch * gain_coeff, bypass_filter, "kq_roll_pitch", updated);
+    2013       12536 :   gains.kq_yaw        = calculateGainChange(dt, gains.kq_yaw, drs_params.kq_yaw * gain_coeff, bypass_filter, "kq_yaw", updated);
+    2014       12536 :   gains.km            = calculateGainChange(dt, gains.km, drs_params.km * gain_coeff, bypass_filter, "km", updated);
+    2015       12536 :   gains.kiwxy         = calculateGainChange(dt, gains.kiwxy, drs_params.kiwxy * gain_coeff, bypass_filter, "kiwxy", updated);
+    2016       12536 :   gains.kibxy         = calculateGainChange(dt, gains.kibxy, drs_params.kibxy * gain_coeff, bypass_filter, "kibxy", updated);
+    2017             : 
+    2018             :   // do not apply muting on these gains
+    2019       12536 :   gains.kiwxy_lim = calculateGainChange(dt, gains.kiwxy_lim, drs_params.kiwxy_lim, false, "kiwxy_lim", updated);
+    2020       12536 :   gains.kibxy_lim = calculateGainChange(dt, gains.kibxy_lim, drs_params.kibxy_lim, false, "kibxy_lim", updated);
+    2021       12536 :   gains.km_lim    = calculateGainChange(dt, gains.km_lim, drs_params.km_lim, false, "km_lim", updated);
+    2022             : 
+    2023       12536 :   mrs_lib::set_mutexed(mutex_gains_, gains, gains_);
+    2024             : 
+    2025             :   // set the gains back to dynamic reconfigure
+    2026             :   // and only do it when some filtering occurs
+    2027       12536 :   if (updated) {
+    2028             : 
+    2029        1201 :     drs_params.kiwxy         = gains.kiwxy;
+    2030        1201 :     drs_params.kibxy         = gains.kibxy;
+    2031        1201 :     drs_params.kq_roll_pitch = gains.kq_roll_pitch;
+    2032        1201 :     drs_params.kq_yaw        = gains.kq_yaw;
+    2033        1201 :     drs_params.km            = gains.km;
+    2034        1201 :     drs_params.km_lim        = gains.km_lim;
+    2035        1201 :     drs_params.kiwxy_lim     = gains.kiwxy_lim;
+    2036        1201 :     drs_params.kibxy_lim     = gains.kibxy_lim;
+    2037             : 
+    2038        1201 :     drs_->updateConfig(drs_params);
+    2039             : 
+    2040        1201 :     ROS_INFO_THROTTLE(10.0, "[%s]: gains have been updated", name_.c_str());
+    2041             :   }
+    2042       12536 : }
+    2043             : 
+    2044             : //}
+    2045             : 
+    2046             : // --------------------------------------------------------------
+    2047             : // |                       other routines                       |
+    2048             : // --------------------------------------------------------------
+    2049             : 
+    2050             : /* calculateGainChange() //{ */
+    2051             : 
+    2052      100288 : double MpcController::calculateGainChange(const double dt, const double current_value, const double desired_value, const bool bypass_rate, std::string name,
+    2053             :                                           bool &updated) {
+    2054             : 
+    2055      100288 :   double change = desired_value - current_value;
+    2056             : 
+    2057      100288 :   double gains_filter_max_change = _gains_filter_change_rate_ * dt;
+    2058      100288 :   double gains_filter_min_change = _gains_filter_min_change_rate_ * dt;
+    2059             : 
+    2060      100288 :   if (!bypass_rate) {
+    2061             : 
+    2062             :     // if current value is near 0...
+    2063             :     double change_in_perc;
+    2064             :     double saturated_change;
+    2065             : 
+    2066       99613 :     if (fabs(current_value) < 1e-6) {
+    2067           0 :       change *= gains_filter_max_change;
+    2068             :     } else {
+    2069             : 
+    2070       99613 :       saturated_change = change;
+    2071             : 
+    2072       99613 :       change_in_perc = (current_value + saturated_change) / current_value - 1.0;
+    2073             : 
+    2074       99613 :       if (change_in_perc > gains_filter_max_change) {
+    2075        4665 :         saturated_change = current_value * gains_filter_max_change;
+    2076       94948 :       } else if (change_in_perc < -gains_filter_max_change) {
+    2077           0 :         saturated_change = current_value * -gains_filter_max_change;
+    2078             :       }
+    2079             : 
+    2080       99613 :       if (fabs(saturated_change) < fabs(change) * gains_filter_min_change) {
+    2081           0 :         change *= gains_filter_min_change;
+    2082             :       } else {
+    2083       99613 :         change = saturated_change;
+    2084             :       }
+    2085             :     }
+    2086             :   }
+    2087             : 
+    2088      100288 :   if (fabs(change) > 1e-3) {
+    2089        6005 :     ROS_DEBUG("[%s]: changing gain '%s' from %.2f to %.2f", name_.c_str(), name.c_str(), current_value, desired_value);
+    2090        6005 :     updated = true;
+    2091             :   }
+    2092             : 
+    2093      100288 :   return current_value + change;
+    2094             : }
+    2095             : 
+    2096             : //}
+    2097             : 
+    2098             : /* getHeadingSafely() //{ */
+    2099             : 
+    2100       49409 : double MpcController::getHeadingSafely(const mrs_msgs::UavState &uav_state, const mrs_msgs::TrackerCommand &tracker_command) {
+    2101             : 
+    2102             :   try {
+    2103       49409 :     return mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    2104             :   }
+    2105           0 :   catch (...) {
+    2106             :   }
+    2107             : 
+    2108             :   try {
+    2109           0 :     return mrs_lib::AttitudeConverter(uav_state.pose.orientation).getYaw();
+    2110             :   }
+    2111           0 :   catch (...) {
+    2112             :   }
+    2113             : 
+    2114           0 :   if (tracker_command.use_heading) {
+    2115           0 :     return tracker_command.heading;
+    2116             :   }
+    2117             : 
+    2118           0 :   return 0;
+    2119             : }
+    2120             : 
+    2121             : //}
+    2122             : 
+    2123             : //}
+    2124             : 
+    2125             : }  // namespace mpc_controller
+    2126             : 
+    2127             : }  // namespace mrs_uav_controllers
+    2128             : 
+    2129             : #include <pluginlib/class_list_macros.h>
+    2130          82 : 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..ec11992da7 --- /dev/null +++ b/mrs_uav_controllers/src/mpc_controller.cpp.gcov.overview.html @@ -0,0 +1,553 @@ + + + + + + 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 + + +
+ 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..b251fcaa81ff67b59bbc573a88ecf1a51e55016c GIT binary patch literal 6652 zcmVe90{{R30vG`D0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpvFwb*Ps6` zquLL?1L?JeG66P0tzEBLz%l(^C^6R|L=lWFf_ld&p~b3|&{!cLnEgHj=KZfD^i24mn%KVGgvk0rg$_ zN8x~9)*ZcH%JM!K;IjIsDDUZ-mJfn4gJgh!vCy@Dc>tDr28;|);8#)rtM9f%HpN(^ zIb+WOCe9QZQWbC=q3LMPLH>u8yceIYm4CSec;Q`;Khi&}?0tLi*C;&&tE7+nRC*Tlv!CHa?GZPNHH!THz)Ud(YBQP3`4?wQqaz6xy+dv+`s}U0Se#i=cOkG4F8~}mN<+)VpM)J zv<2v&iwxs>iW@mbG}lWmV!d;{M4J5uE&MoGzXS@XT&)PuZ&;SNCF|_E26#zJ$BKWO z$xU73N2C`G#)zN7B}gmk;)LL)`HqWr+8kK$UJH7Mnd~RVSp04Vc;3ckYL9IXPpG{W z3594$<{T{8rjg2FbOY7VN3o`*rkCmqo0$TR>-cA8=2Hk9LRMkFP^|4?<|1+4rU)2X zqLazvBKo*2T_-+*v+g>Ak5{e+ByVAOA%{u=ilqQ}igKpIliATv^%DG@&CnTEIzyUQ zi3)_Cq3eQH^ETi*z-A8a!XQ~yTu5$~s#|e9pt_;QV+6WOh!<$&nLSoqpQl05KQ028 zb@=-;vw)6;#i%ew)X!q=N7=OuNJ(7GL<(KUb)^AVX<)=n0_-5B1&E!8HTWMTLB@6d zf#A-oE0x~U)?O8meWT@vYMhT1QZ8i?b=bc2sn4ZpN}2qUk2Jib(vrzMo{+PAy}f< zI9S?d$wgxs8$kMHNte@6?yzMo%v^MDU{TW*)H z76bt58(yzhn7T7@g(AaQdnuqndi;|Q&p&=3lTt8X^hSEzj+S?%RbtREW&j%4^D>O% z0be-A4|{%l*W)+2Z$k`<1N>5AU~zlrF)m1Ma27--!!L2#-hF16pXv?A$Tn_oImT;K zyk9#;gmJfr|2y^A_v%aH@aD7>gdTJ9Zf;oCV%r@Fvaz2;HB|y_VRM)OdPzFuV+yUUOxC zDEFP8PxuiQfj3e1&Q&~{!3lUY>Iv)z)Cw+71X@lfR0XOx>@Jp?a*XM5eQ%7)h8ey@ z-F@=m3hfY$4ZUVv+W|R~wjvP@Qco*KdU8MdI zpak1Jg#+??jxm;{D-OAPj0@O3=cjOEqvB7mUH9yS*EE0)qpJl90ZR&>v;OW zfOsAecjVXc(#~-7Wfq{sfEJ97;wu2^F)H^n*jz7b(?gL0l^C@+#t3-)b9ead;G^U1 z<)#!!{aiN#i-)=y$HxOBrn<&K&nyd|c8ss?x;>AuFLZab2r+>;Y$UG)yl1GbT&Uf{ zf57*rl!<}7czl2*OPRh=t-iFs!O>y59jH_8IctJ{< zst&km0rNbqI|Z~7!<1=!7XjxjaaArxD`ji2yVLF&w0EUaR`XNXFg8dwWBhaG_0)vZ z0SpiPb_K@_28=320oe58+QwMYIA<_6cbaN3Q+GWv_j*`?RVmLjeuC7;xu}#&*zO6? zjFA{H`FwR?tjhQA5egLi9D%+d4%r7i8Rvx+K4MJsTeXHwUAs(pP$wWuZz17iZ2O;n zxYPu^X#xF6TYxB#MgaB1fG4N{2oXqVuZiTMQ?zHC`?xaan1;4o;?4-o3`|trK4a!} zZ7ZeQN%?&teva8gjWH_F-MS3+fvbXxycy#{#=-c9Ph@x;|BmE_`2QY^S4#K~#yw$6 z0XXlOS#W+0O4aO%0x<8a+`8O~!>2H$qC6lYKDehl5u#yn-WijslYBTAD)UKmycJq{83CDTKVD2TCwXfX|q7Bu+uIYj|Dj`iVv$ zb}foVvuhy{D5pAgZ0UMVA`o@-oPAtx*fm^P!mtw%phZ~=fU2%%cnf(rO-D?U1|%Z) zcOs^!pjE`&k2u#G5mOP|j0#Ez0elZ)3Ui|%CT*qOf*73wkci0uUr$WMq-oB~Mgc#> ze278KTL6#t!v1wYDlC96rL8a_h|Fw?GJAeiTw&PFDcLfL-ykvWPTS#veHLRSZ>KL2 zXpF;GKZR2(u#5==NKpX7vGBVy?6Rf(9gn}*{kW2+06Si-X^oKA0_RUrjmNW@sm1Cl z-JaoMo_6NP-6SisziZF+v4EApqG61#k$82t<+>gY4P_RI5@YhZN)ytJ(KS~&3!t*= z{Vt~J9c<<%`6(Xp@RIk>?C*^Zwr}L(C^|H37$1-JSt&K3)|c{2B9OQZSd-+V%DI(l zm_nUQ*f0{0S?MI59#AP0hB5w*6ohh^7i&zCJXie6W-G0x3>R>pJ#Wc{{BKUiyZlY3 zZI);4cc_%Y7)xi)T4yu}U>scm&@#q~p%4Y?*KGl$CKX}4KpjT>$=o80*P&N_vTaII z0S@lapAgSFWH&JzWt0y4U3g@ z9wj5^e9w>ZU>#>@>N@uU$cC|?Dfkp^1wcNR8^f4Xi|p0F7&ijQV9y`4$*MK#G18Mw znfOY8Hwh3JYmgqbP-JIq#q_Tm11uW>{-8}MA+4`yiH(@sO<*D!Zz4wK_GWkE%(@EN zuykFKCIffq@rS8P*P8NLmXPZ5ZveFoof@PnC==ZAfLiIf@l1unI|HCvaN&N8HQh4c z3y=2t_Rv1>6 zxV?`a(KSkn60iXl&XVqosZ*_H&rLK*c~M0FT!(q!@YzvY<;l-4a1B4WFQ?bR_9dp- zfxGYVjmmAMCODEH8}QpS*}5h%oU_~AQm#U?I=`9WjNLLH&oex+`_W+d6S?fBF#Jx_ zL8E>lES-=QKg-wj(Pl-!k2d>gvyV3WXfs9MA8l6d`lHPX?2c8mKH6*y!~c{vW4VB2 zdwsqzpX~8=F84_d093YE<9tcWA7{i1 z7rem1g+S+*I22jfNXA8Ecm}wM6}o%!q+ZaqxE5v+Z<3E&OJp?wPRNP-^Tgz-2zZRu z3}XY}hnQ-cQ=6MJM}Azp0Bbc}q$fr^rX>TG69#y5EM!{%tc}t6LYQ=uCIDthQVjfD z3hQTPa*S^=Ga@s40pDR}GH=AMm>FCVDE>niD37@hslI$XpVjlo2EfV7#}ado#w;IV zesP>@?G&QJ>8@T~IRdsYYP(kN6{G`n=eoYpgfo;rOdlJYV+i(d!vPyrfhRaB>Am*g z9fB;RzS~%XyKadocxEKHA-7_B5(Ei;Wv(kPWW%JZV~i|axsV;60U{pNFs9`BM%PBb za-knR)y-qRo4uZWJ4zBzQ1uGXG2-q~VS@z}u5j4ZRa9U?SBg3{6qkJLHiR;>&GlV|4GDtT8h)gvR8r z${vT*Q{fpZW|2r#XXQG{S3*zz^p#g&Eru5gjXF3Vx4-*;%K>ju zrAJjSB!~Q10nfv55ZV)25l4oGue|XMDIz}`=-kdYFeBX)&UVYbk(^cEh z6dvbTO4G^4_=x4WeYR3cLYgo}li;GK-c>dgAHn?yZnxl0(9AOuccc4sO#1Fi{f{uy zXhniPZqZJ$04lqVmn=s2-ij`-BGWeB?h*ZAr`%!0*sHn2XxHU?Di9!exf->Q2Lc+X zZu!HE?l7G{jGEV#x^gB!m47B!q-;eZ{xGiyUv7~OeYkF)XQlyYp!)O7yz4V_jrrU| z(Enc?8k>KyzxwkI4gZ2;{5s*#@GrQ8zy47;_J!3XWA4zX=*M5TFN{;!>iStKq}jbM zEZ+K~?|SaOaNC$4OCW~t3sDi>c-PQOhenGSJp6mDa`8uS|L=m!JQ{z?p@Dbyc(VCQ z*OTJUxXnxwYnp_Bl06T-yANQ13~0w#xhrB4plyuwfZqs=gpmR#EsmklN1JZRzOI_N z<~fjVjG5oM(%7Bh%XO{G~Bau|dr;ySqY--OqhV~fcw9o!AGK`f=U|83+OJL~7<%swM zKm*myv`^FBEi@F=ji&ju2ah zlR8T(?4gOv#WC_?-UG%6b2AX*Mvf0#00(uwbpZZmsww7i7@@Xz3-E>jM-dZHHUV!5 za8%c!gnJWkKPSIg&jJ)WJf(XS>%ycg`=Q%Xgi$pCOW6o9mQ9 zZg6wMH!?utBds{QC1+}Mqv9*9u&)`7Gw+>uE&I56*I&idIqK%7U30gh*tI~rzU!%O z?sAwSBL-&X8RjH~XC8ml&7JwUyX%|bQ|Gki9Xp*0`;sM{Oo#8FFIXrJr&oO~wG|S4 z{7nxqm^}vO_nEGGsho}K!ebpT$_xhrP1j^T_iqfmtJ1;&l%I+#9X( zN!}%S#268-2N{ZpF%06_DUV0j3Mrp>z2hbbZl6$e80_dGHmni<$t@=XuYvP?tn1v) z$F=g%8utpY3Pp;SxNLwT+~qN=f=t9cJu^yq+%Zxdj}qV)Nt~)+Y>m=4eL~fj&(Lai z&(j(oK{q^uHhX}&dTB1#j6#u7Re0G9|Cqpch4-tg(y`~-GFbL@^Lwrn#SOy02bjbm zGZnJ-!d!YhuL%IZf(1&qqDu*kX*}YWX`B^uDz6>Ys!Eo zjHMG2{TPA5ko^@Ho95lka9+(=kno0PD=Ge*kA>E|e0T#Ol*36|n#ZeC2J=nsX&YDL zRqVHMi~708Pm!nBqTgSJJ(sdwq~zJVstn0D_r>@&JZnbL| z`|KLNwH3QCMqkL3S=XgRKnl2juw@_<0S#0u5&O5(MNG=kti$yrb2hpT z36;&RA))eg*SW`{LqD#`?HhAD2I$A~;hi3;Z?|Gqm5e& zy2b>J6~8-wp)R?t!VP#_`BUK%pcjXF23U@y_H|wG$z*^V0UXjb#&@!` z+L(Y?eT={!4d5mL8i@IKA+}D2D{GciLu|cX*Wwh?Oy@B%q)xz3*IivB%zOJp&dzoH z4w188W=RbyMj~2LxgjMooI25?PE6jMsQ=>-jMj8lJ*2fN?hM4_97Ofrt0U19r_!d<@qCGb3m-bUx3S_!x7!N!^Ghc$1%XjB%uZbvLcUo2Q-X8V57L`Xt)AF4v|;fXW?!VGHm+tHQ$7``lH*Ym)Q2 zB(!{0aOij1T6U}o^|L+>XEoLGO_O&fni)Dtc4umGr4Fjr zSn!77qB4Px{@wm10WdkpJfO#(0pgl9!0-abbg0h&sHOT63&eX@Z}D#XXvB%kNdjjE ztaH(raLStts5qd3J^q;vPjq-}1^)Phnf|b5Iv67H(T5e=#HgtzN6A13i~&4`n&zxr zzh_Gr#9{RMq=sLzmFlp~;7A<$ zXufTBv{8BX#YB7O=RMupn74bbKJ;A&Q(?o4kMwwTBwmC@ZNhZhQ3Ei6cH}BFt}n3# zKf^g{Z^D($@oVzr*IAALhZy(l9-kuh;bc55Y*5%2K1*fxJ9lw6wnsQWGdCXI!?lx< zG}|ybhEtNJ10xq&DTi+dBuNO=zC~Agetn^6+=Kqc^JKXfz%VnyQ%J|^(iofKbBs|V zKEOv+mFm%#)ec{|A{TdZ>UZQsuHNr{K@UG%GIAZHHEvtcmPi + + + + + + 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:62477480.6 %
Date:2024-04-09 22:03:33Functions: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()22
mrs_uav_controllers::se3_controller::Se3Controller::activate(mrs_uav_managers::Controller::ControlOutput const&)27
(anonymous namespace)::ProxyExec0::ProxyExec0()82
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>)82
mrs_uav_controllers::se3_controller::Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig&, unsigned int)164
mrs_uav_controllers::se3_controller::Se3Controller::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)223
mrs_uav_controllers::se3_controller::Se3Controller::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)319
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&)2676
mrs_uav_controllers::se3_controller::Se3Controller::getStatus()2762
mrs_uav_controllers::se3_controller::Se3Controller::timerGains(ros::TimerEvent const&)4359
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&)24094
mrs_uav_controllers::se3_controller::Se3Controller::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)26770
mrs_uav_controllers::se3_controller::Se3Controller::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)26993
mrs_uav_controllers::se3_controller::Se3Controller::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)61026
mrs_uav_controllers::se3_controller::Se3Controller::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)94319
+
+
+ + + +
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..136eb56155 --- /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:62477480.6 %
Date:2024-04-09 22:03:33Functions:151788.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_controllers::se3_controller::Se3Controller::deactivate()22
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>)82
mrs_uav_controllers::se3_controller::Se3Controller::timerGains(ros::TimerEvent const&)4359
mrs_uav_controllers::se3_controller::Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig&, unsigned int)164
mrs_uav_controllers::se3_controller::Se3Controller::updateActive(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)26993
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&)24094
mrs_uav_controllers::se3_controller::Se3Controller::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)319
mrs_uav_controllers::se3_controller::Se3Controller::updateInactive(mrs_msgs::UavState_<std::allocator<void> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)94319
mrs_uav_controllers::se3_controller::Se3Controller::getHeadingSafely(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)26770
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&)2676
mrs_uav_controllers::se3_controller::Se3Controller::calculateGainChange(double, double, double, bool, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool&)61026
mrs_uav_controllers::se3_controller::Se3Controller::positionPassthrough(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_msgs::TrackerCommand_<std::allocator<void> > const&)223
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()2762
+
+
+ + + +
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..22aa4ebbf5 --- /dev/null +++ b/mrs_uav_controllers/src/se3_controller.cpp.gcov.html @@ -0,0 +1,1927 @@ + + + + + + + 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:62477480.6 %
Date:2024-04-09 22:03:33Functions: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          82 : 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          82 :   nh_ = nh;
+     217             : 
+     218          82 :   common_handlers_  = common_handlers;
+     219          82 :   private_handlers_ = private_handlers;
+     220             : 
+     221          82 :   _uav_mass_ = common_handlers->getMass();
+     222             : 
+     223          82 :   ros::Time::waitForValid();
+     224             : 
+     225             :   // | ---------- loading params using the parent's nh ---------- |
+     226             : 
+     227         164 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     228             : 
+     229          82 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     230             : 
+     231          82 :   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          82 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/private/se3_controller.yaml");
+     239          82 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_controllers") + "/config/public/se3_controller.yaml");
+     240             : 
+     241         164 :   const std::string yaml_namespace = "mrs_uav_controllers/se3_controller/";
+     242             : 
+     243             :   // lateral gains
+     244          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kp", gains_.kpxy);
+     245          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kv", gains_.kvxy);
+     246          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/ka", gains_.kaxy);
+     247             : 
+     248          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kiw", gains_.kiwxy);
+     249          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kib", gains_.kibxy);
+     250             : 
+     251             :   // | ------------------------- rampup ------------------------- |
+     252             : 
+     253          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rampup/enabled", _rampup_enabled_);
+     254          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rampup/speed", _rampup_speed_);
+     255             : 
+     256             :   // height gains
+     257          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/kp", gains_.kpz);
+     258          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/kv", gains_.kvz);
+     259          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/vertical/ka", gains_.kaz);
+     260             : 
+     261             :   // attitude gains
+     262          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/attitude/kq_roll_pitch", gains_.kq_roll_pitch);
+     263          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/attitude/kq_yaw", gains_.kq_yaw);
+     264             : 
+     265             :   // attitude rate gains
+     266          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/attitude_rate_gains/kw_roll_pitch", gains_.kw_roll_pitch);
+     267          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/attitude_rate_gains/kw_yaw", gains_.kw_yaw);
+     268             : 
+     269             :   // mass estimator
+     270          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/mass_estimator/km", gains_.km);
+     271          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/mass_estimator/km_lim", gains_.km_lim);
+     272             : 
+     273             :   // integrator limits
+     274          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kiw_lim", gains_.kiwxy_lim);
+     275          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/default_gains/horizontal/kib_lim", gains_.kibxy_lim);
+     276             : 
+     277             :   // constraints
+     278          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/tilt_angle_failsafe/enabled", _tilt_angle_failsafe_enabled_);
+     279          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/tilt_angle_failsafe/limit", _tilt_angle_failsafe_);
+     280             : 
+     281          82 :   _tilt_angle_failsafe_ = M_PI * (_tilt_angle_failsafe_ / 180.0);
+     282             : 
+     283          82 :   if (_tilt_angle_failsafe_enabled_ && fabs(_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          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/constraints/throttle_saturation", _throttle_saturation_);
+     289             : 
+     290             :   // gain filtering
+     291          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/perc_change_rate", _gains_filter_change_rate_);
+     292          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/min_change_rate", _gains_filter_min_change_rate_);
+     293          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/rate", _gain_filtering_rate_);
+     294          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/gain_filtering/gain_mute_coefficient", _gain_mute_coefficient_);
+     295             : 
+     296             :   // output mode
+     297          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/preferred_output", drs_params_.preferred_output_mode);
+     298             : 
+     299          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/rotation_matrix", drs_params_.rotation_type);
+     300             : 
+     301             :   // angular rate feed forward
+     302         164 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/angular_rate_feedforward/parasitic_pitch_roll",
+     303          82 :                                             drs_params_.pitch_roll_heading_rate_compensation);
+     304          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "se3/angular_rate_feedforward/jerk", drs_params_.jerk_feedforward);
+     305             : 
+     306             :   // | ------------------- position pid params ------------------ |
+     307             : 
+     308          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/p", _pos_pid_p_);
+     309          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/i", _pos_pid_i_);
+     310          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/translation_gains/d", _pos_pid_d_);
+     311             : 
+     312          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/p", _hdg_pid_p_);
+     313          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/i", _hdg_pid_i_);
+     314          82 :   private_handlers->param_loader->loadParam(yaml_namespace + "position_controller/heading_gains/d", _hdg_pid_d_);
+     315             : 
+     316             :   // | ------------------ finish loading params ----------------- |
+     317             : 
+     318          82 :   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          82 :   if (!(drs_params_.preferred_output_mode == OUTPUT_ACTUATORS || drs_params_.preferred_output_mode == OUTPUT_CONTROL_GROUP ||
+     326          82 :         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          82 :   uav_mass_difference_ = 0;
+     333          82 :   Iw_w_                = Eigen::Vector2d::Zero(2);
+     334          82 :   Ib_b_                = Eigen::Vector2d::Zero(2);
+     335             : 
+     336             :   // | --------------- dynamic reconfigure server --------------- |
+     337             : 
+     338          82 :   drs_params_.kpxy             = gains_.kpxy;
+     339          82 :   drs_params_.kvxy             = gains_.kvxy;
+     340          82 :   drs_params_.kaxy             = gains_.kaxy;
+     341          82 :   drs_params_.kiwxy            = gains_.kiwxy;
+     342          82 :   drs_params_.kibxy            = gains_.kibxy;
+     343          82 :   drs_params_.kpz              = gains_.kpz;
+     344          82 :   drs_params_.kvz              = gains_.kvz;
+     345          82 :   drs_params_.kaz              = gains_.kaz;
+     346          82 :   drs_params_.kq_roll_pitch    = gains_.kq_roll_pitch;
+     347          82 :   drs_params_.kq_yaw           = gains_.kq_yaw;
+     348          82 :   drs_params_.kiwxy_lim        = gains_.kiwxy_lim;
+     349          82 :   drs_params_.kibxy_lim        = gains_.kibxy_lim;
+     350          82 :   drs_params_.km               = gains_.km;
+     351          82 :   drs_params_.km_lim           = gains_.km_lim;
+     352          82 :   drs_params_.jerk_feedforward = true;
+     353             : 
+     354          82 :   drs_.reset(new Drs_t(mutex_drs_, nh_));
+     355          82 :   drs_->updateConfig(drs_params_);
+     356          82 :   Drs_t::CallbackType f = boost::bind(&Se3Controller::callbackDrs, this, _1, _2);
+     357          82 :   drs_->setCallback(f);
+     358             : 
+     359             :   // | ------------------------- timers ------------------------- |
+     360             : 
+     361          82 :   timer_gains_ = nh_.createTimer(ros::Rate(_gain_filtering_rate_), &Se3Controller::timerGains, this, false, false);
+     362             : 
+     363             :   // | ---------------------- position pid ---------------------- |
+     364             : 
+     365          82 :   position_pid_x_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     366          82 :   position_pid_y_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     367          82 :   position_pid_z_.setParams(_pos_pid_p_, _pos_pid_d_, _pos_pid_i_, -1, 1.0);
+     368          82 :   position_pid_heading_.setParams(_hdg_pid_p_, _hdg_pid_d_, _hdg_pid_i_, -1, 0.1);
+     369             : 
+     370             :   // | ------------------------ profiler ------------------------ |
+     371             : 
+     372          82 :   profiler_ = mrs_lib::Profiler(common_handlers_->parent_nh, "Se3Controller", _profiler_enabled_);
+     373             : 
+     374             :   // | ----------------------- finish init ---------------------- |
+     375             : 
+     376          82 :   ROS_INFO("[Se3Controller]: initialized");
+     377             : 
+     378          82 :   is_initialized_ = true;
+     379             : 
+     380          82 :   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_ = fabs(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          22 : void Se3Controller::deactivate(void) {
+     461             : 
+     462          22 :   is_active_           = false;
+     463          22 :   first_iteration_     = false;
+     464          22 :   uav_mass_difference_ = 0;
+     465             : 
+     466          22 :   timer_gains_.stop();
+     467             : 
+     468          22 :   ROS_INFO("[Se3Controller]: deactivated");
+     469          22 : }
+     470             : 
+     471             : //}
+     472             : 
+     473             : /* updateInactive() //{ */
+     474             : 
+     475       94319 : void Se3Controller::updateInactive(const mrs_msgs::UavState& uav_state, [[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& tracker_command) {
+     476             : 
+     477       94319 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     478             : 
+     479       94319 :   last_update_time_ = uav_state.header.stamp;
+     480             : 
+     481       94319 :   first_iteration_ = false;
+     482       94319 : }
+     483             : 
+     484             : //}
+     485             : 
+     486             : /* //{ updateWhenActive() */
+     487             : 
+     488       26993 : Se3Controller::ControlOutput Se3Controller::updateActive(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+     489             : 
+     490       80979 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateActive");
+     491       80979 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("Se3Controller::updateActive", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     492             : 
+     493       53986 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     494             : 
+     495       26993 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     496             : 
+     497       26993 :   last_control_output_.desired_heading_rate          = {};
+     498       26993 :   last_control_output_.desired_orientation           = {};
+     499       26993 :   last_control_output_.desired_unbiased_acceleration = {};
+     500       26993 :   last_control_output_.control_output                = {};
+     501             : 
+     502             :   // | -------------------- calculate the dt -------------------- |
+     503             : 
+     504             :   double dt;
+     505             : 
+     506       26993 :   if (first_iteration_) {
+     507          27 :     dt               = 0.01;
+     508          27 :     first_iteration_ = false;
+     509             :   } else {
+     510       26966 :     dt = (uav_state.header.stamp - last_update_time_).toSec();
+     511             :   }
+     512             : 
+     513       26993 :   last_update_time_ = uav_state.header.stamp;
+     514             : 
+     515       26993 :   if (fabs(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       26993 :   auto lowest_modality = common::getLowestOuput(common_handlers_->control_output_modalities);
+     525             : 
+     526       26993 :   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       26993 :   if (drs_params.preferred_output_mode == OUTPUT_ATTITUDE_RATE && common_handlers_->control_output_modalities.attitude_rate) {
+     536       19517 :     ROS_DEBUG_THROTTLE(1.0, "[Se3Controller]: prioritizing attitude rate output");
+     537       19517 :     lowest_modality = common::ATTITUDE_RATE;
+     538        7476 :   } 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        7476 :   } 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        7476 :   } 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       26993 :   switch (lowest_modality.value()) {
+     550             : 
+     551         223 :     case common::POSITION: {
+     552         223 :       positionPassthrough(uav_state, tracker_command);
+     553         223 :       break;
+     554             :     }
+     555             : 
+     556         928 :     case common::VELOCITY_HDG: {
+     557         928 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG, dt);
+     558         928 :       break;
+     559             :     }
+     560             : 
+     561        1748 :     case common::VELOCITY_HDG_RATE: {
+     562        1748 :       PIDVelocityOutput(uav_state, tracker_command, common::VELOCITY_HDG_RATE, dt);
+     563        1748 :       break;
+     564             :     }
+     565             : 
+     566         497 :     case common::ACCELERATION_HDG: {
+     567         497 :       SE3Controller(uav_state, tracker_command, dt, common::ACCELERATION_HDG);
+     568         497 :       break;
+     569             :     }
+     570             : 
+     571         497 :     case common::ACCELERATION_HDG_RATE: {
+     572         497 :       SE3Controller(uav_state, tracker_command, dt, common::ACCELERATION_HDG_RATE);
+     573         497 :       break;
+     574             :     }
+     575             : 
+     576        1131 :     case common::ATTITUDE: {
+     577        1131 :       SE3Controller(uav_state, tracker_command, dt, common::ATTITUDE);
+     578        1131 :       break;
+     579             :     }
+     580             : 
+     581       19517 :     case common::ATTITUDE_RATE: {
+     582       19517 :       SE3Controller(uav_state, tracker_command, dt, common::ATTITUDE_RATE);
+     583       19517 :       break;
+     584             :     }
+     585             : 
+     586        1231 :     case common::CONTROL_GROUP: {
+     587        1231 :       SE3Controller(uav_state, tracker_command, dt, common::CONTROL_GROUP);
+     588        1231 :       break;
+     589             :     }
+     590             : 
+     591        1221 :     case common::ACTUATORS_CMD: {
+     592        1221 :       SE3Controller(uav_state, tracker_command, dt, common::ACTUATORS_CMD);
+     593        1221 :       break;
+     594             :     }
+     595             : 
+     596       26993 :     default: {
+     597             :     }
+     598             :   }
+     599             : 
+     600       26993 :   return last_control_output_;
+     601             : }
+     602             : 
+     603             : //}
+     604             : 
+     605             : /* //{ getStatus() */
+     606             : 
+     607        2762 : const mrs_msgs::ControllerStatus Se3Controller::getStatus() {
+     608             : 
+     609        2762 :   mrs_msgs::ControllerStatus controller_status;
+     610             : 
+     611        2762 :   controller_status.active = is_active_;
+     612             : 
+     613        2762 :   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         319 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr Se3Controller::setConstraints([
+     674             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& constraints) {
+     675             : 
+     676         319 :   if (!is_initialized_) {
+     677           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+     678             :   }
+     679             : 
+     680         319 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+     681             : 
+     682         319 :   ROS_INFO("[Se3Controller]: updating constraints");
+     683             : 
+     684         638 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     685         319 :   res.success = true;
+     686         319 :   res.message = "constraints updated";
+     687             : 
+     688         319 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     689             : }
+     690             : 
+     691             : //}
+     692             : 
+     693             : // --------------------------------------------------------------
+     694             : // |                         controllers                        |
+     695             : // --------------------------------------------------------------
+     696             : 
+     697             : /* SE3Controller() //{ */
+     698             : 
+     699       24094 : 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       48188 :   auto drs_params  = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+     703       24094 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+     704       24094 :   auto gains       = mrs_lib::get_mutexed(mutex_gains_, gains_);
+     705             : 
+     706             :   // | ----------------- get the current heading ---------------- |
+     707             : 
+     708       24094 :   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       24094 :   Eigen::Vector3d Rp = Eigen::Vector3d::Zero(3);
+     719       24094 :   Eigen::Vector3d Rv = Eigen::Vector3d::Zero(3);
+     720       24094 :   Eigen::Vector3d Ra = Eigen::Vector3d::Zero(3);
+     721             : 
+     722       24094 :   if (tracker_command.use_position_vertical || tracker_command.use_position_horizontal) {
+     723             : 
+     724       24094 :     if (tracker_command.use_position_horizontal) {
+     725       24094 :       Rp[0] = tracker_command.position.x;
+     726       24094 :       Rp[1] = tracker_command.position.y;
+     727             :     } else {
+     728           0 :       Rv[0] = 0;
+     729           0 :       Rv[1] = 0;
+     730             :     }
+     731             : 
+     732       24094 :     if (tracker_command.use_position_vertical) {
+     733       24094 :       Rp[2] = tracker_command.position.z;
+     734             :     } else {
+     735           0 :       Rv[2] = 0;
+     736             :     }
+     737             :   }
+     738             : 
+     739       24094 :   if (tracker_command.use_velocity_horizontal) {
+     740       24094 :     Rv[0] = tracker_command.velocity.x;
+     741       24094 :     Rv[1] = tracker_command.velocity.y;
+     742             :   } else {
+     743           0 :     Rv[0] = 0;
+     744           0 :     Rv[1] = 0;
+     745             :   }
+     746             : 
+     747       24094 :   if (tracker_command.use_velocity_vertical) {
+     748       24094 :     Rv[2] = tracker_command.velocity.z;
+     749             :   } else {
+     750           0 :     Rv[2] = 0;
+     751             :   }
+     752             : 
+     753       24094 :   if (tracker_command.use_acceleration) {
+     754       24093 :     Ra << tracker_command.acceleration.x, tracker_command.acceleration.y, tracker_command.acceleration.z;
+     755             :   } else {
+     756           1 :     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       24094 :   Eigen::Vector3d Op(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+     764       24094 :   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       24094 :   Eigen::Matrix3d R = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+     768             : 
+     769             :   // Ow - UAV angular rate
+     770       24094 :   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       24094 :   Eigen::Vector3d Ep(0, 0, 0);
+     776             : 
+     777       24094 :   if (tracker_command.use_position_horizontal || tracker_command.use_position_vertical) {
+     778       24094 :     Ep = Rp - Op;
+     779             :   }
+     780             : 
+     781             :   // velocity control error
+     782       24094 :   Eigen::Vector3d Ev(0, 0, 0);
+     783             : 
+     784       24094 :   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       24094 :     Ev = Rv - Ov;
+     787             :   }
+     788             : 
+     789             :   // | --------------------- load the gains --------------------- |
+     790             : 
+     791       24094 :   mute_gains_by_tracker_ = tracker_command.disable_position_gains;
+     792             : 
+     793       24094 :   Eigen::Vector3d Ka(0, 0, 0);
+     794       24094 :   Eigen::Array3d  Kp(0, 0, 0);
+     795       24094 :   Eigen::Array3d  Kv(0, 0, 0);
+     796       24094 :   Eigen::Array3d  Kq(0, 0, 0);
+     797       24094 :   Eigen::Array3d  Kw(0, 0, 0);
+     798             : 
+     799             :   {
+     800       24094 :     std::scoped_lock lock(mutex_gains_);
+     801             : 
+     802       24094 :     if (tracker_command.use_position_horizontal) {
+     803       24094 :       Kp[0] = gains.kpxy;
+     804       24094 :       Kp[1] = gains.kpxy;
+     805             :     } else {
+     806           0 :       Kp[0] = 0;
+     807           0 :       Kp[1] = 0;
+     808             :     }
+     809             : 
+     810       24094 :     if (tracker_command.use_position_vertical) {
+     811       24094 :       Kp[2] = gains.kpz;
+     812             :     } else {
+     813           0 :       Kp[2] = 0;
+     814             :     }
+     815             : 
+     816       24094 :     if (tracker_command.use_velocity_horizontal) {
+     817       24094 :       Kv[0] = gains.kvxy;
+     818       24094 :       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       24094 :     if (tracker_command.use_velocity_vertical || tracker_command.use_position_vertical) {
+     826       24094 :       Kv[2] = gains.kvz;
+     827             :     } else {
+     828           0 :       Kv[2] = 0;
+     829             :     }
+     830             : 
+     831       24094 :     if (tracker_command.use_acceleration) {
+     832       24093 :       Ka << gains.kaxy, gains.kaxy, gains.kaz;
+     833             :     } else {
+     834           1 :       Ka << 0, 0, 0;
+     835             :     }
+     836             : 
+     837       24094 :     if (!tracker_command.use_attitude_rate) {
+     838       24094 :       Kq << gains.kq_roll_pitch, gains.kq_roll_pitch, gains.kq_yaw;
+     839             :     }
+     840             : 
+     841       24094 :     Kw[0] = gains.kw_roll_pitch;
+     842       24094 :     Kw[1] = gains.kw_roll_pitch;
+     843       24094 :     Kw[2] = gains.kw_yaw;
+     844             :   }
+     845             : 
+     846       24094 :   Kp = Kp * (_uav_mass_ + uav_mass_difference_);
+     847       24094 :   Kv = Kv * (_uav_mass_ + uav_mass_difference_);
+     848             : 
+     849             :   // | --------------- desired orientation matrix --------------- |
+     850             : 
+     851             :   // get body integral in the world frame
+     852             : 
+     853       24094 :   Eigen::Vector2d Ib_w = Eigen::Vector2d(0, 0);
+     854             : 
+     855             :   {
+     856       48188 :     geometry_msgs::Vector3Stamped Ib_b_stamped;
+     857             : 
+     858       24094 :     Ib_b_stamped.header.stamp    = ros::Time::now();
+     859       24094 :     Ib_b_stamped.header.frame_id = "fcu_untilted";
+     860       24094 :     Ib_b_stamped.vector.x        = Ib_b_(0);
+     861       24094 :     Ib_b_stamped.vector.y        = Ib_b_(1);
+     862       24094 :     Ib_b_stamped.vector.z        = 0;
+     863             : 
+     864       48188 :     auto res = common_handlers_->transformer->transformSingle(Ib_b_stamped, uav_state_.header.frame_id);
+     865             : 
+     866       24094 :     if (res) {
+     867       24094 :       Ib_w[0] = res.value().vector.x;
+     868       24094 :       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       24094 :   double total_mass = _uav_mass_ + uav_mass_difference_;
+     877             : 
+     878       24094 :   Eigen::Vector3d feed_forward      = total_mass * (Eigen::Vector3d(0, 0, common_handlers_->g) + Ra);
+     879       24094 :   Eigen::Vector3d position_feedback = Kp * Ep.array();
+     880       24094 :   Eigen::Vector3d velocity_feedback = Kv * Ev.array();
+     881       24094 :   Eigen::Vector3d integral_feedback;
+     882             :   {
+     883       24094 :     std::scoped_lock lock(mutex_integrals_);
+     884             : 
+     885       24094 :     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       48188 :     std::scoped_lock lock(mutex_gains_, mutex_integrals_);
+     900             : 
+     901       24094 :     Eigen::Vector3d integration_switch(1, 1, 0);
+     902             : 
+     903             :     // integrate the world error
+     904       24094 :     if (tracker_command.use_position_horizontal) {
+     905       24094 :       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       24094 :     bool world_integral_saturated = false;
+     912       24094 :     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       24094 :     } else if (Iw_w_[0] > gains.kiwxy_lim) {
+     916           0 :       Iw_w_[0]                 = gains.kiwxy_lim;
+     917           0 :       world_integral_saturated = true;
+     918       24094 :     } 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       24094 :     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       24094 :     world_integral_saturated = false;
+     929       24094 :     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       24094 :     } else if (Iw_w_[1] > gains.kiwxy_lim) {
+     933           0 :       Iw_w_[1]                 = gains.kiwxy_lim;
+     934           0 :       world_integral_saturated = true;
+     935       24094 :     } 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       24094 :     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       48188 :     std::scoped_lock lock(mutex_gains_);
+     955             : 
+     956       24094 :     Eigen::Vector2d Ep_fcu_untilted = Eigen::Vector2d(0, 0);  // position error in the untilted frame of the UAV
+     957       24094 :     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       48188 :       geometry_msgs::Vector3Stamped Ep_stamped;
+     963             : 
+     964       24094 :       Ep_stamped.header.stamp    = ros::Time::now();
+     965       24094 :       Ep_stamped.header.frame_id = uav_state_.header.frame_id;
+     966       24094 :       Ep_stamped.vector.x        = Ep(0);
+     967       24094 :       Ep_stamped.vector.y        = Ep(1);
+     968       24094 :       Ep_stamped.vector.z        = Ep(2);
+     969             : 
+     970       72282 :       auto res = common_handlers_->transformer->transformSingle(Ep_stamped, "fcu_untilted");
+     971             : 
+     972       24094 :       if (res) {
+     973       24094 :         Ep_fcu_untilted[0] = res.value().vector.x;
+     974       24094 :         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       48188 :       geometry_msgs::Vector3Stamped Ev_stamped;
+     983             : 
+     984       24094 :       Ev_stamped.header.stamp    = ros::Time::now();
+     985       24094 :       Ev_stamped.header.frame_id = uav_state_.header.frame_id;
+     986       24094 :       Ev_stamped.vector.x        = Ev(0);
+     987       24094 :       Ev_stamped.vector.y        = Ev(1);
+     988       24094 :       Ev_stamped.vector.z        = Ev(2);
+     989             : 
+     990       72282 :       auto res = common_handlers_->transformer->transformSingle(Ev_stamped, "fcu_untilted");
+     991             : 
+     992       24094 :       if (res) {
+     993       24094 :         Ev_fcu_untilted[0] = res.value().vector.x;
+     994       24094 :         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       24094 :     if (tracker_command.use_position_horizontal) {
+    1002       24094 :       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       24094 :     bool body_integral_saturated = false;
+    1009       24094 :     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       24094 :     } else if (Ib_b_[0] > gains.kibxy_lim) {
+    1013           0 :       Ib_b_[0]                = gains.kibxy_lim;
+    1014           0 :       body_integral_saturated = true;
+    1015       24094 :     } 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       24094 :     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       24094 :     body_integral_saturated = false;
+    1026       24094 :     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       24094 :     } else if (Ib_b_[1] > gains.kibxy_lim) {
+    1030           0 :       Ib_b_[1]                = gains.kibxy_lim;
+    1031           0 :       body_integral_saturated = true;
+    1032       24094 :     } 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       24094 :     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       24094 :   if (output_modality == common::ACCELERATION_HDG || output_modality == common::ACCELERATION_HDG_RATE) {
+    1045             : 
+    1046         994 :     Eigen::Vector3d des_acc = (position_feedback + velocity_feedback + integral_feedback) / total_mass + Ra;
+    1047             : 
+    1048         994 :     if (output_modality == common::ACCELERATION_HDG) {
+    1049             : 
+    1050         994 :       mrs_msgs::HwApiAccelerationHdgCmd cmd;
+    1051             : 
+    1052         497 :       cmd.acceleration.x = des_acc[0];
+    1053         497 :       cmd.acceleration.y = des_acc[1];
+    1054         497 :       cmd.acceleration.z = des_acc[2];
+    1055             : 
+    1056         497 :       cmd.heading = tracker_command.heading;
+    1057             : 
+    1058         497 :       last_control_output_.control_output = cmd;
+    1059             : 
+    1060             :     } else {
+    1061             : 
+    1062         497 :       double des_hdg_ff = 0;
+    1063             : 
+    1064         497 :       if (tracker_command.use_heading_rate) {
+    1065         497 :         des_hdg_ff = tracker_command.heading_rate;
+    1066             :       }
+    1067             : 
+    1068         994 :       mrs_msgs::HwApiAccelerationHdgRateCmd cmd;
+    1069             : 
+    1070         497 :       cmd.acceleration.x = des_acc[0];
+    1071         497 :       cmd.acceleration.y = des_acc[1];
+    1072         497 :       cmd.acceleration.z = des_acc[2];
+    1073             : 
+    1074         497 :       position_pid_heading_.setSaturation(constraints.heading_speed);
+    1075             : 
+    1076         497 :       double hdg_err = mrs_lib::geometry::sradians::diff(tracker_command.heading, uav_heading);
+    1077             : 
+    1078         497 :       double des_hdg_rate = position_pid_heading_.update(hdg_err, dt) + des_hdg_ff;
+    1079             : 
+    1080         497 :       cmd.heading_rate = des_hdg_rate;
+    1081             : 
+    1082         497 :       last_control_output_.desired_heading_rate = des_hdg_rate;
+    1083             : 
+    1084         497 :       last_control_output_.control_output = cmd;
+    1085             :     }
+    1086             : 
+    1087             :     // | -------------- unbiased desired acceleration ------------- |
+    1088             : 
+    1089         994 :     Eigen::Vector3d unbiased_des_acc(0, 0, 0);
+    1090             : 
+    1091             :     {
+    1092             : 
+    1093         994 :       Eigen::Vector3d unbiased_des_acc_world = (position_feedback + velocity_feedback) / total_mass + Ra;
+    1094             : 
+    1095        1988 :       geometry_msgs::Vector3Stamped world_accel;
+    1096             : 
+    1097         994 :       world_accel.header.stamp    = ros::Time::now();
+    1098         994 :       world_accel.header.frame_id = uav_state.header.frame_id;
+    1099         994 :       world_accel.vector.x        = unbiased_des_acc_world[0];
+    1100         994 :       world_accel.vector.y        = unbiased_des_acc_world[1];
+    1101         994 :       world_accel.vector.z        = unbiased_des_acc_world[2];
+    1102             : 
+    1103        2982 :       auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1104             : 
+    1105         994 :       if (res) {
+    1106         994 :         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         994 :     last_control_output_.desired_unbiased_acceleration = unbiased_des_acc;
+    1112             : 
+    1113             :     // | ----------------- fill in the diagnostics ---------------- |
+    1114             : 
+    1115         994 :     last_control_output_.diagnostics.ramping_up = false;
+    1116             : 
+    1117         994 :     last_control_output_.diagnostics.mass_estimator  = false;
+    1118         994 :     last_control_output_.diagnostics.mass_difference = 0;
+    1119         994 :     last_control_output_.diagnostics.total_mass      = total_mass;
+    1120             : 
+    1121         994 :     last_control_output_.diagnostics.disturbance_estimator = true;
+    1122             : 
+    1123         994 :     last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_[0];
+    1124         994 :     last_control_output_.diagnostics.disturbance_by_b = -Ib_b_[1];
+    1125             : 
+    1126         994 :     last_control_output_.diagnostics.disturbance_bx_w = -Ib_w[0];
+    1127         994 :     last_control_output_.diagnostics.disturbance_by_w = -Ib_w[1];
+    1128             : 
+    1129         994 :     last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_[0];
+    1130         994 :     last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_[1];
+    1131             : 
+    1132         994 :     last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1133             : 
+    1134         994 :     last_control_output_.diagnostics.controller = "Se3Controller";
+    1135             : 
+    1136         994 :     return;
+    1137             :   }
+    1138             : 
+    1139             :   /* mass estimatior //{ */
+    1140             : 
+    1141             :   // --------------------------------------------------------------
+    1142             :   // |                integrate the mass difference               |
+    1143             :   // --------------------------------------------------------------
+    1144             : 
+    1145             :   {
+    1146       46200 :     std::scoped_lock lock(mutex_gains_);
+    1147             : 
+    1148       23100 :     if (tracker_command.use_position_vertical && !rampup_active_) {
+    1149       23085 :       uav_mass_difference_ += gains.km * Ep[2] * dt;
+    1150             :     }
+    1151             : 
+    1152             :     // saturate the mass estimator
+    1153       23100 :     bool uav_mass_saturated = false;
+    1154       23100 :     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       23100 :     } else if (uav_mass_difference_ > gains.km_lim) {
+    1158           0 :       uav_mass_difference_ = gains.km_lim;
+    1159           0 :       uav_mass_saturated   = true;
+    1160       23100 :     } 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       23100 :     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       23100 :   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       23100 :   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       23100 :   double tilt_safety_limit = _tilt_angle_failsafe_enabled_ ? _tilt_angle_failsafe_ : std::numeric_limits<double>::max();
+    1188             : 
+    1189       23100 :   auto f_normed_sanitized = common::sanitizeDesiredForce(f.normalized(), tilt_safety_limit, constraints.tilt, "Se3Controller");
+    1190             : 
+    1191       23100 :   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       23100 :   Eigen::Vector3d f_normed = f_normed_sanitized.value();
+    1205             : 
+    1206             :   // --------------------------------------------------------------
+    1207             :   // |               desired orientation + throttle               |
+    1208             :   // --------------------------------------------------------------
+    1209             : 
+    1210             :   // | ------------------- desired orientation ------------------ |
+    1211             : 
+    1212       23100 :   Eigen::Matrix3d Rd;
+    1213             : 
+    1214       23100 :   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       23100 :     Eigen::Vector3d bxd;  // desired heading vector
+    1231             : 
+    1232       23100 :     if (tracker_command.use_heading) {
+    1233       23100 :       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       23100 :     Rd = common::so3transform(f_normed, bxd, drs_params.rotation_type == 1);
+    1240             :   }
+    1241             : 
+    1242             :   // | -------------------- desired throttle -------------------- |
+    1243             : 
+    1244       23100 :   double desired_thrust_force = f.dot(R.col(2));
+    1245       23100 :   double throttle             = 0;
+    1246             : 
+    1247       23100 :   if (tracker_command.use_throttle) {
+    1248             : 
+    1249             :     // the throttle is overriden from the tracker command
+    1250           0 :     throttle = tracker_command.throttle;
+    1251             : 
+    1252       23100 :   } else if (rampup_active_) {
+    1253             : 
+    1254             :     // deactivate the rampup when the times up
+    1255          15 :     if (fabs((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       23085 :     if (desired_thrust_force >= 0) {
+    1277       23085 :       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       23100 :   bool throttle_saturated = false;
+    1286             : 
+    1287       23100 :   if (!std::isfinite(throttle)) {
+    1288             : 
+    1289           0 :     ROS_ERROR("[Se3Controller]: NaN detected in variable 'throttle'!!!");
+    1290           0 :     return;
+    1291             : 
+    1292       23100 :   } 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       23100 :   } 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       23100 :   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       23100 :   Eigen::Vector3d unbiased_des_acc(0, 0, 0);
+    1321             : 
+    1322             :   {
+    1323       23100 :     Eigen::Vector3d unbiased_des_acc_world = (position_feedback + velocity_feedback) / total_mass + Ra;
+    1324             : 
+    1325       46200 :     geometry_msgs::Vector3Stamped world_accel;
+    1326             : 
+    1327       23100 :     world_accel.header.stamp    = ros::Time::now();
+    1328       23100 :     world_accel.header.frame_id = uav_state.header.frame_id;
+    1329       23100 :     world_accel.vector.x        = unbiased_des_acc_world[0];
+    1330       23100 :     world_accel.vector.y        = unbiased_des_acc_world[1];
+    1331       23100 :     world_accel.vector.z        = unbiased_des_acc_world[2];
+    1332             : 
+    1333       69300 :     auto res = common_handlers_->transformer->transformSingle(world_accel, "fcu");
+    1334             : 
+    1335       23100 :     if (res) {
+    1336       23100 :       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       23100 :   last_control_output_.desired_orientation = mrs_lib::AttitudeConverter(Rd);
+    1344             : 
+    1345             :   // fill the unbiased desired accelerations
+    1346       23100 :   last_control_output_.desired_unbiased_acceleration = unbiased_des_acc;
+    1347             : 
+    1348             :   // | ----------------- fill in the diagnostics ---------------- |
+    1349             : 
+    1350       23100 :   last_control_output_.diagnostics.ramping_up = rampup_active_;
+    1351             : 
+    1352       23100 :   last_control_output_.diagnostics.mass_estimator  = true;
+    1353       23100 :   last_control_output_.diagnostics.mass_difference = uav_mass_difference_;
+    1354       23100 :   last_control_output_.diagnostics.total_mass      = total_mass;
+    1355             : 
+    1356       23100 :   last_control_output_.diagnostics.disturbance_estimator = true;
+    1357             : 
+    1358       23100 :   last_control_output_.diagnostics.disturbance_bx_b = -Ib_b_[0];
+    1359       23100 :   last_control_output_.diagnostics.disturbance_by_b = -Ib_b_[1];
+    1360             : 
+    1361       23100 :   last_control_output_.diagnostics.disturbance_bx_w = -Ib_w[0];
+    1362       23100 :   last_control_output_.diagnostics.disturbance_by_w = -Ib_w[1];
+    1363             : 
+    1364       23100 :   last_control_output_.diagnostics.disturbance_wx_w = -Iw_w_[0];
+    1365       23100 :   last_control_output_.diagnostics.disturbance_wy_w = -Iw_w_[1];
+    1366             : 
+    1367       23100 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1368             : 
+    1369       23100 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1370             : 
+    1371             :   // | ------------ construct the attitude reference ------------ |
+    1372             : 
+    1373       23100 :   mrs_msgs::HwApiAttitudeCmd attitude_cmd;
+    1374             : 
+    1375       23100 :   attitude_cmd.stamp       = ros::Time::now();
+    1376       23100 :   attitude_cmd.orientation = mrs_lib::AttitudeConverter(Rd);
+    1377       23100 :   attitude_cmd.throttle    = throttle;
+    1378             : 
+    1379       23100 :   if (output_modality == common::ATTITUDE) {
+    1380             : 
+    1381        1131 :     last_control_output_.control_output = attitude_cmd;
+    1382             : 
+    1383        1131 :     return;
+    1384             :   }
+    1385             : 
+    1386             :   // --------------------------------------------------------------
+    1387             :   // |                      attitude control                      |
+    1388             :   // --------------------------------------------------------------
+    1389             : 
+    1390       21969 :   Eigen::Vector3d rate_feedforward = Eigen::Vector3d::Zero(3);
+    1391             : 
+    1392       21969 :   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       21969 :   } else if (tracker_command.use_heading_rate) {
+    1397             : 
+    1398             :     // to fill in the feed forward yaw rate
+    1399       21968 :     double desired_yaw_rate = 0;
+    1400             : 
+    1401             :     try {
+    1402       21968 :       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       21968 :     rate_feedforward << 0, 0, desired_yaw_rate;
+    1409             :   }
+    1410             : 
+    1411             :   // | ------------ jerk feedforward -> angular rate ------------ |
+    1412             : 
+    1413       21969 :   Eigen::Vector3d jerk_feedforward = Eigen::Vector3d(0, 0, 0);
+    1414             : 
+    1415       21969 :   if (tracker_command.use_jerk && drs_params.jerk_feedforward) {
+    1416             : 
+    1417       21958 :     ROS_DEBUG_THROTTLE(1.0, "[Se3Controller]: using jerk feedforward");
+    1418             : 
+    1419       21958 :     Eigen::Matrix3d I;
+    1420       21958 :     I << 0, 1, 0, -1, 0, 0, 0, 0, 0;
+    1421       21958 :     Eigen::Vector3d desired_jerk = Eigen::Vector3d(tracker_command.jerk.x, tracker_command.jerk.y, tracker_command.jerk.z);
+    1422       21958 :     jerk_feedforward             = (I.transpose() * Rd.transpose() * desired_jerk) / (desired_thrust_force / total_mass);
+    1423             :   }
+    1424             : 
+    1425             :   // | --------------- run the attitude controller -------------- |
+    1426             : 
+    1427       21969 :   Eigen::Vector3d attitude_rate_saturation(constraints.roll_rate, constraints.pitch_rate, constraints.yaw_rate);
+    1428             : 
+    1429       21969 :   auto attitude_rate_command = common::attitudeController(uav_state, attitude_cmd, jerk_feedforward + rate_feedforward, attitude_rate_saturation, Kq,
+    1430       43938 :                                                           drs_params.pitch_roll_heading_rate_compensation);
+    1431             : 
+    1432       21969 :   if (!attitude_rate_command) {
+    1433           0 :     return;
+    1434             :   }
+    1435             : 
+    1436             :   // | --------- fill in the already known attitude rate -------- |
+    1437             : 
+    1438             :   {
+    1439             :     try {
+    1440       21969 :       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       21969 :   if (output_modality == common::ATTITUDE_RATE) {
+    1449             : 
+    1450       19517 :     last_control_output_.control_output = attitude_rate_command;
+    1451             : 
+    1452       19517 :     return;
+    1453             :   }
+    1454             : 
+    1455             :   // --------------------------------------------------------------
+    1456             :   // |                    Attitude rate control                   |
+    1457             :   // --------------------------------------------------------------
+    1458             : 
+    1459        2452 :   Kw = common_handlers_->detailed_model_params->inertia.diagonal().array() * Kw;
+    1460             : 
+    1461        2452 :   auto control_group_command = common::attitudeRateController(uav_state, attitude_rate_command.value(), Kw);
+    1462             : 
+    1463        2452 :   if (!control_group_command) {
+    1464           0 :     return;
+    1465             :   }
+    1466             : 
+    1467        2452 :   if (output_modality == common::CONTROL_GROUP) {
+    1468             : 
+    1469        1231 :     last_control_output_.control_output = control_group_command;
+    1470             : 
+    1471        1231 :     return;
+    1472             :   }
+    1473             : 
+    1474             :   // --------------------------------------------------------------
+    1475             :   // |                        output mixer                        |
+    1476             :   // --------------------------------------------------------------
+    1477             : 
+    1478        2442 :   mrs_msgs::HwApiActuatorCmd actuator_cmd = common::actuatorMixer(control_group_command.value(), common_handlers_->detailed_model_params->control_group_mixer);
+    1479             : 
+    1480        1221 :   last_control_output_.control_output = actuator_cmd;
+    1481             : 
+    1482        1221 :   return;
+    1483             : }
+    1484             : 
+    1485             : //}
+    1486             : 
+    1487             : /* positionPassthrough() //{ */
+    1488             : 
+    1489         223 : void Se3Controller::positionPassthrough(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+    1490             : 
+    1491         223 :   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         446 :   mrs_msgs::HwApiPositionCmd cmd;
+    1497             : 
+    1498         223 :   cmd.header.frame_id = uav_state.header.frame_id;
+    1499         223 :   cmd.header.stamp    = ros::Time::now();
+    1500             : 
+    1501         223 :   cmd.position = tracker_command.position;
+    1502         223 :   cmd.heading  = tracker_command.heading;
+    1503             : 
+    1504         223 :   last_control_output_.control_output = cmd;
+    1505             : 
+    1506             :   // fill the unbiased desired accelerations
+    1507         223 :   last_control_output_.desired_unbiased_acceleration = {};
+    1508         223 :   last_control_output_.desired_orientation           = {};
+    1509         223 :   last_control_output_.desired_heading_rate          = {};
+    1510             : 
+    1511             :   // | ----------------- fill in the diagnostics ---------------- |
+    1512             : 
+    1513         223 :   last_control_output_.diagnostics.ramping_up = false;
+    1514             : 
+    1515         223 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1516         223 :   last_control_output_.diagnostics.mass_difference = 0;
+    1517             : 
+    1518         223 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1519             : 
+    1520         223 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1521         223 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1522             : 
+    1523         223 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1524         223 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1525             : 
+    1526         223 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1527         223 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1528             : 
+    1529         223 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1530             : 
+    1531         223 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1532             : }
+    1533             : 
+    1534             : //}
+    1535             : 
+    1536             : /* PIDVelocityOutput() //{ */
+    1537             : 
+    1538        2676 : 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        2676 :   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        2676 :   auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1547        2676 :   auto gains       = mrs_lib::get_mutexed(mutex_gains_, gains_);
+    1548             : 
+    1549        2676 :   Eigen::Vector3d pos_ref = Eigen::Vector3d(tracker_command.position.x, tracker_command.position.y, tracker_command.position.z);
+    1550        2676 :   Eigen::Vector3d pos     = Eigen::Vector3d(uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z);
+    1551             : 
+    1552        2676 :   double hdg_ref = tracker_command.heading;
+    1553        2676 :   double hdg     = getHeadingSafely(uav_state, tracker_command);
+    1554             : 
+    1555             :   // | ------------------ velocity feedforward ------------------ |
+    1556             : 
+    1557        2676 :   Eigen::Vector3d vel_ff(0, 0, 0);
+    1558             : 
+    1559        2676 :   if (tracker_command.use_velocity_horizontal && tracker_command.use_velocity_vertical) {
+    1560        2676 :     vel_ff = Eigen::Vector3d(tracker_command.velocity.x, tracker_command.velocity.y, tracker_command.velocity.z);
+    1561             :   }
+    1562             : 
+    1563             :   // | -------------------------- gains ------------------------- |
+    1564             : 
+    1565        2676 :   Eigen::Vector3d Kp;
+    1566             : 
+    1567             :   {
+    1568        2676 :     std::scoped_lock lock(mutex_gains_);
+    1569             : 
+    1570        2676 :     Kp << gains.kpxy, gains.kpxy, gains.kpz;
+    1571             :   }
+    1572             : 
+    1573             :   // | --------------------- control errors --------------------- |
+    1574             : 
+    1575        2676 :   Eigen::Vector3d Ep = pos_ref - pos;
+    1576             : 
+    1577             :   // | --------------------------- pid -------------------------- |
+    1578             : 
+    1579        2676 :   position_pid_x_.setSaturation(constraints.horizontal_speed);
+    1580        2676 :   position_pid_y_.setSaturation(constraints.horizontal_speed);
+    1581        2676 :   position_pid_z_.setSaturation(std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed));
+    1582             : 
+    1583        2676 :   double des_vel_x = position_pid_x_.update(Ep[0], dt);
+    1584        2676 :   double des_vel_y = position_pid_y_.update(Ep[1], dt);
+    1585        2676 :   double des_vel_z = position_pid_z_.update(Ep[2], dt);
+    1586             : 
+    1587             :   // | -------------------- position feedback ------------------- |
+    1588             : 
+    1589        2676 :   Eigen::Vector3d des_vel = Eigen::Vector3d(des_vel_x, des_vel_y, des_vel_z) + vel_ff;
+    1590             : 
+    1591        2676 :   if (control_output == common::VELOCITY_HDG) {
+    1592             : 
+    1593             :     // | --------------------- fill the output -------------------- |
+    1594             : 
+    1595        1856 :     mrs_msgs::HwApiVelocityHdgCmd cmd;
+    1596             : 
+    1597         928 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1598         928 :     cmd.header.stamp    = ros::Time::now();
+    1599             : 
+    1600         928 :     cmd.velocity.x = des_vel[0];
+    1601         928 :     cmd.velocity.y = des_vel[1];
+    1602         928 :     cmd.velocity.z = des_vel[2];
+    1603             : 
+    1604         928 :     cmd.heading = tracker_command.heading;
+    1605             : 
+    1606         928 :     last_control_output_.control_output = cmd;
+    1607             : 
+    1608        1748 :   } else if (control_output == common::VELOCITY_HDG_RATE) {
+    1609             : 
+    1610        1748 :     position_pid_heading_.setSaturation(constraints.heading_speed);
+    1611             : 
+    1612        1748 :     double hdg_err = mrs_lib::geometry::sradians::diff(hdg_ref, hdg);
+    1613             : 
+    1614        1748 :     double des_hdg_rate = position_pid_heading_.update(hdg_err, dt);
+    1615             : 
+    1616             :     // | --------------------------- ff --------------------------- |
+    1617             : 
+    1618        1748 :     double des_hdg_ff = 0;
+    1619             : 
+    1620        1748 :     if (tracker_command.use_heading_rate) {
+    1621        1748 :       des_hdg_ff = tracker_command.heading_rate;
+    1622             :     }
+    1623             : 
+    1624             :     // | --------------------- fill the output -------------------- |
+    1625             : 
+    1626        3496 :     mrs_msgs::HwApiVelocityHdgRateCmd cmd;
+    1627             : 
+    1628        1748 :     cmd.header.frame_id = uav_state.header.frame_id;
+    1629        1748 :     cmd.header.stamp    = ros::Time::now();
+    1630             : 
+    1631        1748 :     cmd.velocity.x = des_vel[0];
+    1632        1748 :     cmd.velocity.y = des_vel[1];
+    1633        1748 :     cmd.velocity.z = des_vel[2];
+    1634             : 
+    1635        1748 :     cmd.heading_rate = des_hdg_rate + des_hdg_ff;
+    1636             : 
+    1637        1748 :     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        2676 :   last_control_output_.desired_unbiased_acceleration = {};
+    1646        2676 :   last_control_output_.desired_orientation           = {};
+    1647        2676 :   last_control_output_.desired_heading_rate          = {};
+    1648             : 
+    1649             :   // | ----------------- fill in the diagnostics ---------------- |
+    1650             : 
+    1651        2676 :   last_control_output_.diagnostics.ramping_up = false;
+    1652             : 
+    1653        2676 :   last_control_output_.diagnostics.mass_estimator  = false;
+    1654        2676 :   last_control_output_.diagnostics.mass_difference = 0;
+    1655             : 
+    1656        2676 :   last_control_output_.diagnostics.disturbance_estimator = false;
+    1657             : 
+    1658        2676 :   last_control_output_.diagnostics.disturbance_bx_b = 0;
+    1659        2676 :   last_control_output_.diagnostics.disturbance_by_b = 0;
+    1660             : 
+    1661        2676 :   last_control_output_.diagnostics.disturbance_bx_w = 0;
+    1662        2676 :   last_control_output_.diagnostics.disturbance_by_w = 0;
+    1663             : 
+    1664        2676 :   last_control_output_.diagnostics.disturbance_wx_w = 0;
+    1665        2676 :   last_control_output_.diagnostics.disturbance_wy_w = 0;
+    1666             : 
+    1667        2676 :   last_control_output_.diagnostics.controller_enforcing_constraints = false;
+    1668             : 
+    1669        2676 :   last_control_output_.diagnostics.controller = "Se3Controller";
+    1670             : }
+    1671             : 
+    1672             : //}
+    1673             : 
+    1674             : // --------------------------------------------------------------
+    1675             : // |                          callbacks                         |
+    1676             : 
+    1677             : 
+    1678             : /* //{ callbackDrs() */
+    1679             : 
+    1680         164 : void Se3Controller::callbackDrs(mrs_uav_controllers::se3_controllerConfig& config, [[maybe_unused]] uint32_t level) {
+    1681             : 
+    1682         164 :   mrs_lib::set_mutexed(mutex_drs_params_, config, drs_params_);
+    1683             : 
+    1684         164 :   ROS_INFO("[Se3Controller]: DRS updated gains");
+    1685         164 : }
+    1686             : 
+    1687             : //}
+    1688             : 
+    1689             : // --------------------------------------------------------------
+    1690             : // |                           timers                           |
+    1691             : // --------------------------------------------------------------
+    1692             : 
+    1693             : /* timerGains() //{ */
+    1694             : 
+    1695        4359 : void Se3Controller::timerGains(const ros::TimerEvent& event) {
+    1696             : 
+    1697       13077 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerGains", _gain_filtering_rate_, 1.0, event);
+    1698             :   mrs_lib::ScopeTimer timer =
+    1699       13077 :       mrs_lib::ScopeTimer("Se3Controller::timerGains", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1700             : 
+    1701        8718 :   auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+    1702        4359 :   auto gains      = mrs_lib::get_mutexed(mutex_gains_, gains_);
+    1703             : 
+    1704             :   // When muting the gains, we want to bypass the filter,
+    1705             :   // so it happens immediately.
+    1706        4359 :   bool   bypass_filter = (mute_gains_ || mute_gains_by_tracker_);
+    1707        4359 :   double gain_coeff    = (mute_gains_ || mute_gains_by_tracker_) ? _gain_mute_coefficient_ : 1.0;
+    1708             : 
+    1709        4359 :   mute_gains_ = false;
+    1710             : 
+    1711        4359 :   const double dt = (event.current_real - event.last_real).toSec();
+    1712             : 
+    1713        4359 :   bool updated = false;
+    1714             : 
+    1715        4359 :   gains.kpxy          = calculateGainChange(dt, gains.kpxy, drs_params.kpxy * gain_coeff, bypass_filter, "kpxy", updated);
+    1716        4359 :   gains.kvxy          = calculateGainChange(dt, gains.kvxy, drs_params.kvxy * gain_coeff, bypass_filter, "kvxy", updated);
+    1717        4359 :   gains.kaxy          = calculateGainChange(dt, gains.kaxy, drs_params.kaxy * gain_coeff, bypass_filter, "kaxy", updated);
+    1718        4359 :   gains.kiwxy         = calculateGainChange(dt, gains.kiwxy, drs_params.kiwxy * gain_coeff, bypass_filter, "kiwxy", updated);
+    1719        4359 :   gains.kibxy         = calculateGainChange(dt, gains.kibxy, drs_params.kibxy * gain_coeff, bypass_filter, "kibxy", updated);
+    1720        4359 :   gains.kpz           = calculateGainChange(dt, gains.kpz, drs_params.kpz * gain_coeff, bypass_filter, "kpz", updated);
+    1721        4359 :   gains.kvz           = calculateGainChange(dt, gains.kvz, drs_params.kvz * gain_coeff, bypass_filter, "kvz", updated);
+    1722        4359 :   gains.kaz           = calculateGainChange(dt, gains.kaz, drs_params.kaz * gain_coeff, bypass_filter, "kaz", updated);
+    1723        4359 :   gains.kq_roll_pitch = calculateGainChange(dt, gains.kq_roll_pitch, drs_params.kq_roll_pitch * gain_coeff, bypass_filter, "kq_roll_pitch", updated);
+    1724        4359 :   gains.kq_yaw        = calculateGainChange(dt, gains.kq_yaw, drs_params.kq_yaw * gain_coeff, bypass_filter, "kq_yaw", updated);
+    1725        4359 :   gains.km            = calculateGainChange(dt, gains.km, drs_params.km * gain_coeff, bypass_filter, "km", updated);
+    1726             : 
+    1727             :   // do not apply muting on these gains
+    1728        4359 :   gains.kiwxy_lim = calculateGainChange(dt, gains.kiwxy_lim, drs_params.kiwxy_lim, false, "kiwxy_lim", updated);
+    1729        4359 :   gains.kibxy_lim = calculateGainChange(dt, gains.kibxy_lim, drs_params.kibxy_lim, false, "kibxy_lim", updated);
+    1730        4359 :   gains.km_lim    = calculateGainChange(dt, gains.km_lim, drs_params.km_lim, false, "km_lim", updated);
+    1731             : 
+    1732        4359 :   mrs_lib::set_mutexed(mutex_gains_, gains, gains_);
+    1733             : 
+    1734             :   // set the gains back to dynamic reconfigure
+    1735             :   // and only do it when some filtering occurs
+    1736        4359 :   if (updated) {
+    1737             : 
+    1738         243 :     drs_params.kpxy          = gains.kpxy;
+    1739         243 :     drs_params.kvxy          = gains.kvxy;
+    1740         243 :     drs_params.kaxy          = gains.kaxy;
+    1741         243 :     drs_params.kiwxy         = gains.kiwxy;
+    1742         243 :     drs_params.kibxy         = gains.kibxy;
+    1743         243 :     drs_params.kpz           = gains.kpz;
+    1744         243 :     drs_params.kvz           = gains.kvz;
+    1745         243 :     drs_params.kaz           = gains.kaz;
+    1746         243 :     drs_params.kq_roll_pitch = gains.kq_roll_pitch;
+    1747         243 :     drs_params.kq_yaw        = gains.kq_yaw;
+    1748         243 :     drs_params.kiwxy_lim     = gains.kiwxy_lim;
+    1749         243 :     drs_params.kibxy_lim     = gains.kibxy_lim;
+    1750         243 :     drs_params.km            = gains.km;
+    1751         243 :     drs_params.km_lim        = gains.km_lim;
+    1752             : 
+    1753         243 :     drs_->updateConfig(drs_params);
+    1754             : 
+    1755         243 :     ROS_INFO_THROTTLE(10.0, "[Se3Controller]: gains have been updated");
+    1756             :   }
+    1757        4359 : }
+    1758             : 
+    1759             : //}
+    1760             : 
+    1761             : // --------------------------------------------------------------
+    1762             : // |                       other routines                       |
+    1763             : // --------------------------------------------------------------
+    1764             : 
+    1765             : /* calculateGainChange() //{ */
+    1766             : 
+    1767       61026 : double Se3Controller::calculateGainChange(const double dt, const double current_value, const double desired_value, const bool bypass_rate, std::string name,
+    1768             :                                           bool& updated) {
+    1769             : 
+    1770       61026 :   double change = desired_value - current_value;
+    1771             : 
+    1772       61026 :   double gains_filter_max_change = _gains_filter_change_rate_ * dt;
+    1773       61026 :   double gains_filter_min_change = _gains_filter_min_change_rate_ * dt;
+    1774             : 
+    1775       61026 :   if (!bypass_rate) {
+    1776             : 
+    1777             :     // if current value is near 0...
+    1778             :     double change_in_perc;
+    1779             :     double saturated_change;
+    1780             : 
+    1781       60729 :     if (fabs(current_value) < 1e-6) {
+    1782           0 :       change *= gains_filter_max_change;
+    1783             :     } else {
+    1784             : 
+    1785       60729 :       saturated_change = change;
+    1786             : 
+    1787       60729 :       change_in_perc = (current_value + saturated_change) / current_value - 1.0;
+    1788             : 
+    1789       60729 :       if (change_in_perc > gains_filter_max_change) {
+    1790        2079 :         saturated_change = current_value * gains_filter_max_change;
+    1791       58650 :       } else if (change_in_perc < -gains_filter_max_change) {
+    1792          27 :         saturated_change = current_value * -gains_filter_max_change;
+    1793             :       }
+    1794             : 
+    1795       60729 :       if (fabs(saturated_change) < fabs(change) * gains_filter_min_change) {
+    1796          21 :         change *= gains_filter_min_change;
+    1797             :       } else {
+    1798       60708 :         change = saturated_change;
+    1799             :       }
+    1800             :     }
+    1801             :   }
+    1802             : 
+    1803       61026 :   if (fabs(change) > 1e-3) {
+    1804        2694 :     ROS_DEBUG("[Se3Controller]: changing gain '%s' from %.2f to %.2f", name.c_str(), current_value, desired_value);
+    1805        2694 :     updated = true;
+    1806             :   }
+    1807             : 
+    1808       61026 :   return current_value + change;
+    1809             : }
+    1810             : 
+    1811             : //}
+    1812             : 
+    1813             : /* getHeadingSafely() //{ */
+    1814             : 
+    1815       26770 : double Se3Controller::getHeadingSafely(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) {
+    1816             : 
+    1817             :   try {
+    1818       26770 :     return mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1819             :   }
+    1820           0 :   catch (...) {
+    1821             :   }
+    1822             : 
+    1823             :   try {
+    1824           0 :     return mrs_lib::AttitudeConverter(uav_state.pose.orientation).getYaw();
+    1825             :   }
+    1826           0 :   catch (...) {
+    1827             :   }
+    1828             : 
+    1829           0 :   if (tracker_command.use_heading) {
+    1830           0 :     return tracker_command.heading;
+    1831             :   }
+    1832             : 
+    1833           0 :   return 0;
+    1834             : }
+    1835             : 
+    1836             : //}
+    1837             : 
+    1838             : }  // namespace se3_controller
+    1839             : 
+    1840             : }  // namespace mrs_uav_controllers
+    1841             : 
+    1842             : #include <pluginlib/class_list_macros.h>
+    1843          82 : 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..363703df5f --- /dev/null +++ b/mrs_uav_controllers/src/se3_controller.cpp.gcov.overview.html @@ -0,0 +1,481 @@ + + + + + + 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 + + +
+ 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..2ace4bff43fb43de19c56956a3e32b50baa5da09 GIT binary patch literal 5838 zcmV;<7BT6GP)A#hW%_j)~63-?x7j|#YaTI}s0DZvax?Hc<_4Vg6 zs=V->NUt@N5wHrXt$LLLj;Z&8iMb9TiowW}#Z_H`*H&G?f&j%B8C>LTETs+UvFmT{OJEp7IeA#6$k^a9&mNvr$`B)tDcdgYg`o&#src8dW@N>S4cHiZx^5ibyB)qG2kTt zE;*t-F$9q5n>g*QuIdcOF~%&r8H@|tfk_*)fNOP}YRiPFdGW%0iuMSMeu`Sx$kDDT zxIE2bNMB!EZx}5Dp)M6wBfap1id)dI#EcRmq#u!|CaDK(*j53;Okn z`?O)|BN9*nJRXKk7?GGI)W3A8(y$BCFIM!K9SbY{aSrf8t3Y4kFIMuo-uP>fo{SFB z#(gY33;4I4;b@Y|%*;(oToED0xOfTbv`J}oT*avBCDU4c)f0LK7OySk0GZ#l^U@Ks zgnAdNy!v-d#EAT6K)r721|H+u#SIQ)XywJ*UGAUN>yj%0boPRu@fQ!-RdosQ5+{0~ z=C<>c2y`nNh>?sLVhMDNnH98=RT{{!vJJ=p;$L)&C8F95E6|Ld!4-QsC>GJ&(BLH< zL7o=rU@o*V``ZMtUu$*@4~<{Fe6%zCLfV+YaUDNqW{$!**~dO_9))YYnF(MwNdZGm zcreLVJNo;oCYn8^tmx7fw6O?Ceo-^2>Q$GCX`h-;XYLhv|J;&!%>hO)>Oy86|6z>e zq*S9}&`fn)go}RcYm;N8Qgz@>Q5>$&(k+n~b(*R?N--k2ss!U!N!8+l`r7$tyOj@_ z8)`h@Uuz@m?Kc8uVaNLGPmD8s_Qeu|QgLMhic)OPs>Y~kqpz}=PF!xc{f3MGY+iq2 zxoo)5UqwK1*>2`l$5t)0>H?r|#1@)f0W<-SZHTt&9kXQs#v&a`jD8MZuff(?g066F znZrRFa{{q*(j%ps;)y+_!N@mlr-1kS4^tk;-^b7M^~aXG1$@1K-pl23uR(Y~d4bFD z>bnl81U+3?EDAtHU)$m52jR~2lu=6fH51uZPz{dbJee?uE-G-V;m zW=qdxT@yEG^f6QOY2zKno_!H!+>bE#oYB5^W(;xcF~H~JpDx_sgsYr?W|z3<$+3f< z5BJY|m$;xw)mA0w(AW#Usnd3K&)8~h2W%KKgVM-0+$Pt~l>C{12C--Wpg~nLe4Gd9 zpkaUK01+_>e~{|BK?&*x+#oTGq&KzkCsFkn+B+#%IpyW*i?+jF9RQ2Q9NS zfYLU;yXtyBB2&uTz#0Su;uMN=60lyE`s5N;B9LvCLQUDT4Aa{eupXkyHaz)c7rQBe zoTxdXY7Oo!WncTrGe?Pv7>E%FW`bOW5lqJ>32*~InEz13XfOv~HZiM*-=h=Y`79Xt zFTlgaA~5YWZIrP`T$nlh9I|^Hyhf*+1&r2=5&*)PstjnPfLl8EVL&Z0WHJq(0^qzQ zuE@oprI-r48}*(EcHtTgt}fSSBDWc{Sx0q+iE500PGO&#jyHhez=e}#D|Fcj0joYz zT^ntue~(ar z zEBf&{eRtsOV~Xg{&gpS4av|zE*Sre{S<~g?Dla-fM{HET}a$L9%B$G z_g$e2f8ucbdEO)7GeWTpuT|}z>s8wc;D@W46^&}ue5MOmwf%>sny-3JGADM+^bXez zI&uq37;x?$=6L&2#1sUVCj}GpSv`~K z8!0ijk0C}e0dT}X!1ohVP-$d6W4^?EiOCToG58c!5BDGuRUu#XuX4o?J4-uR9pMKc zot;i;Hlwl$V5bew+pN?^YUSh4nP}jU7Xh)223pWqwdML)z^s+16beovPyI!ij+vbM;TbmFod5p>hS&G_hykT85+R&| znCWK>4Oo*h3(^lAnhmpAStfKC@g$q+Dmmb<>g$0~`+Q^+YNs$=x%d-#)#4}sb;Z-- zyZ2x1W8c#cJqo+$CIxK67!c<+Rn<01AizLj22j(+LhL9A)UPuExWb+4cJT+h!8dbH zb#p59DC?E!KtrDP8@mMxPZ+VU#U&(Ou<^&ZYcqx_S-?2Z9NNkAkANI&m7-E0XOjNqu2%z+O6Tb;_auKu8%62Eg)Q+aODNJHvuJP@f# zBMpZz)^7LgDk1sO}XV>dLE@qthaZWX=z_GWJ9X$=mUZQ2hybE zIjGbZ*wX-5o&Y9nsybCO%jR3YtqMS4nQ&0#B_92<1bbDD1hXdyRaEvEO?Il@!^w3^ z)fl76p2C<^P0!9?0pq!jm8zA1`Aq|Is`tvtN(Xbb`lBd&b|b^3xC252s9Cv^ur^A^ zIiKI@p1r_ekj>odtRuv^94m82oHiL}4oQM}x4u0Lcs7AcdF=qqEFkh#fitBtqfm{} z>SLA=@D!HeNiGseyfDVj{Tj2N?4<}OHVcN68-XM=?A6)9N zP^{N*mMS@~m3ASD$Ft5ccN!UYDaXqu!PduNrZ~Tw%@SqLIh$w2t<}%~-E-!Sdt|#% zvTHCban&Tv{91TES_Yqca>0ReaV=(6TaVG#5E64~OQ?@DyOW=5o)+X#}gLuVeE$zeWQntU8>e3+{G_=LsS%jux|xcYSnuKG0z#CC>**s?Hv71OUB5 zqq>ED7@&gcn(IU9`H;CjPK+!QdjLhQk8@82ziJ}a$GHzlG&fI>@bQCd`;M6kpn~e} znEBK(;|%elU%$r?w+|_M+6{60K-n|bo+*M+L%c9%ew{s2+^CIuEw_2E*`CQBASSPR zu03N6@xL&{L^CElJi^A^qkrBlxS;vzeD@XHuiz5F4VL)dGQ^Ra0`i8Oh1h6mLgR7A zPfb~NBv)srfMSfv{#4T&zDg9Se+Z~+BhDiqfe|s1;iSe{F#0$N+eRN8DH9{7T9Q!} z)oW@5Fo2?{rk3iujQ>pu&zEqKmkMJHL+45hMmiFGCmHpO8{LI*-!TuU#)z6mGl!bF%Hwx+`8koX`aptK1^gQ5vGRl*r@>TVZ20bnz{OP?uW1n1&Wt=Mvj{Z z{Y`$^;-ye`Hz3BrA9#LhDNoT*xYt@~i#!qNZc}2a)z@;6p8gKSx8@)XKQ$ftd#!A` z|A@f%*j)KLy3=tp}|P4}i#_un%f0Uy$&exT+6#aHB{9Kz2nSE` z@(2eU=4u?DE%BM!rN4TNVUl3ei$5e`faa`((o8ko353u$VFX4p52(VJ%gt`b7<)QC zxa2bXX@)0m#_9^KGgG^@u@c?0RJW{!V5xihTcEAk_2vT@5{tcCVVAf_+t-=my?Zi@ z+#TI8iSvAU_qMB4YgVo^$oY_-n3gVa+0;v3%Dn#da5g@vy0FBBs_P27pz8bdqj^j2 zxKbMtYYTvnptwHC1yn?grege|-kBDR6lV>io2i~TjI@Pip#Z-d!5$j37{DIf%%_5w ziO$qP*-y-IGWmz1Ab@uxoIQaS{l}=3OKmrF9`oGPL-rN&UVH#Z`~Z(8{zi491pR z*9^@Dq&^h^=~m2K8)=5tfHwNt2Vx}Ws44&lRc#soe>2qxb3ew^8Q3sf!D;^?=ssY$ z=F{Ce$%mm5-n(P6U$~H1Ppt#+Da*TXFzHM_m#`mS;6$^j+otH!KYFHdIR8`9+tS#M@g8QxzwCm`mDHlWj)lSAeO#MTfwU*V?#MlL+Hc^@!b>8RKvpco_C#||E1 z9MUvmten_>darFSo8k%YiN&6~EfWx9f9U~9jN~>qoC*)q>9~}Lb5p?5;HN;LWsZxx z03!fz72Ld+^qqN9z6((TO-vnZv9IO7(60(e7O0R+Sry(M8p0MpOv`NP+nwD%nf<Bv>_J5rPC0rulrg*al4M6WBJq=MHe=P~L@f9e1d!wEE%#5zVfPZ=pLhYz zYRpBh2w*-8*H(4;M)U}15a5uiA->L2i^c?U%5C)QkpLPFP(jSU3k@|gT)26^7#eDI z&t|(2r+1EteKZ1ot8S_qV4f85`DF|Hpk4Xw!k(_{?81(nKmN2GartW+fTf1D%REiN zirZiu4nal1)k-)yjWEHCcZl(T+9d58b8;A)qe5?v~5vQ5CVMtR zjF@VnZI9xGqqabKe(y|$lL2*YtR3Oo@9^!x2y;HjdXDbC>S-fsaTCTwIg9u_$_9+Q zo*7u$JY28gAep}5y1OQpiIG#CYU;VmaueTo+N%L-+vrjH3*-l2 zM2u$uW!p3r@|f8+_~SQpo^KxKm4ENdr}%n?^X2OqZo~NhbkBZ0!`sJrhS`!y1Gu2? zjxs(w3bt|aTHY9pd{~+k-IhE~ByPcj+r58g59byo3%gj%#8rQem@PDIWa`Ok_J`~d zGEF5FvzU_vRi_4_D5ufyFb|-Vp%twUegMRx&o^UZ3Sd0MkOs30;?^`~{NdA!b$MEi z#_t6soxQ>h-`OsFwQ<`KqseVhuxBjU8w=>WN!!<@6F#);agnzj^bGDKy|W;?3X0Q^ zmg@64jS`yJ>S=(62L$_Nlz=8vHOS(h1sFckHb!U>lkoh+c)VqCm}5d_e=Ac+#t0Ad(gRBe=% z)F+j@ZnOKwAjfS-wd#7_^twi`WSQjvaEKvC?y1O9?=~3Y z{o%Y*`n>CWXc`%@2m5*IE+H7rL*zyl`C-ZytAlCHDsWhyThu!+e!cCljn@d z>Jky-@DwdDHJ69$?{XoJa-H4S^?Juos<}tH5gFzxl!}oHg>WPCoKU5ZYP)dJb4^H5 z9m4oVs)ZLe+*7JEO_|ch^dNo~qp)U0)#E3Cb%9q&jhF!l#uh7rlq=K + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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..4c8cab219e --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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..703fe50830 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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..965e3dc477 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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..1157ccf786 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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..269fff9602 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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..df89dc4db6 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/output_publisher.h.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager/output_publisher.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_manager - output_publisher.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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&)477
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)997
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1042
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1181
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2253
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3816
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3845
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)7897
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)78813
mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::PublisherVisitor(mrs_uav_managers::control_manager::OutputPublisher*)100321
+
+
+ + + +
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..5b71d70ce0 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/output_publisher.h.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager/output_publisher.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_manager - output_publisher.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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*)100321
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > >(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3816
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)7897
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiPositionCmd_<std::allocator<void> > >(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)477
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1181
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)78813
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > >(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3845
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1042
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2253
void mrs_uav_managers::control_manager::OutputPublisher::PublisherVisitor::operator()<mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > >(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)997
+
+
+ + + +
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..8614411cd7 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.html @@ -0,0 +1,146 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager/output_publisher.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/control_manager - output_publisher.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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      100321 :     PublisherVisitor(OutputPublisher* obj) : obj_(obj){};
+      48             : 
+      49             :     OutputPublisher* obj_;
+      50             : 
+      51             :     template <class T>
+      52      100321 :     void operator()(const T& msg) {
+      53      100321 :       obj_->publish(msg);
+      54      100321 :     }
+      55             :   };
+      56             : };
+      57             : 
+      58             : }  // namespace control_manager
+      59             : 
+      60             : }  // namespace mrs_uav_managers
+      61             : 
+      62             : #endif  //  CONTROL_MANAGER_OUTPUT_PUBLISHER
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.overview.html b/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.overview.html new file mode 100644 index 0000000000..09b263e5e1 --- /dev/null +++ b/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.overview.html @@ -0,0 +1,36 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/control_manager/output_publisher.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.png b/mrs_uav_managers/include/control_manager/output_publisher.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..fdd688bb370f7663153724647927eaa459667bca GIT binary patch literal 332 zcmeAS@N?(olHy`uVBq!ia0vp^0YGfW!VDz+4mNH9QW60^A+G=b|6c_J%iqVwzWUF= zunH&+rp|e9UJ7J$7I;J!GcfQS0b$0e+I-SL!CRg#jv*eMTc_R@JgmUO63D)Y^##MP z_qxsoW@77=HNHQc`DTrZj!g2}RPCgyMn{X24zmu;3tZ^#7E%+I9$1z2-~Hw84U=k2 z4|j*$Z0-BVwQ#LZPe@1*@1FIJ-4-|f*Lgnua4f;i^kDBLO$82VM}aqiJexoA?e=!; zP1$C<>VaG4@4cUR8vfjSmptKVZex36aP6U&;q4|#;dgdw2qk^7v)`$HtkLlNGdUR# zYu`;D4^40T_+7%T&EUnw&}9A@Oy}y>9psSfzqj9wQ#d|**7eZ4D=rjGy76rCwIcb5 X-I2 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - agl_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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&)61
mrs_uav_managers::AglEstimator::~AglEstimator().261
+
+
+ + + +
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..66294098e2 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - agl_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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&)61
mrs_uav_managers::AglEstimator::~AglEstimator()0
mrs_uav_managers::AglEstimator::~AglEstimator().261
+
+
+ + + +
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..66b3827606 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h.gcov.html @@ -0,0 +1,157 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/agl_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - agl_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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          61 :   AglEstimator(const std::string &name, const std::string &frame_id, const std::string &package_name)
+      55          61 :       : Estimator(agl::type, name, frame_id), package_name_(package_name) {
+      56          61 :   }
+      57             : 
+      58          61 :   virtual ~AglEstimator(void) {
+      59          61 :   }
+      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..5a17eab164 --- /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-04-09 22:03:33Functions: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&)477
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)477
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&)997
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)997
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&)1042
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1042
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&)1181
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1181
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&)2253
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2382
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&)3816
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&)3845
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)5209
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)5238
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&)7897
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)13432
mrs_uav_managers::control_manager::HwApiInitializeVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >&, mrs_msgs::UavState_<std::allocator<void> > const&, double const&, double const&)13719
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&)65096
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)81397
+
+
+ + + +
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..59b6b44715 --- /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-04-09 22:03:33Functions: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&)3816
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&)7897
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&)477
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&)1181
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&)65096
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&)3845
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&)1042
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&)2253
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&)997
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&)13719
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&)5209
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)13432
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)477
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1181
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)81397
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)5238
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1042
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2382
mrs_uav_managers::control_manager::HwApiCmdExtractThrottleVisitor::operator()(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)997
+
+
+ + + +
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..0a45ceaa70 --- /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-04-09 22:03:33Functions: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        5209 :   std::optional<double> operator()(const mrs_msgs::HwApiActuatorCmd& msg) {
+      82             : 
+      83        5209 :     if (msg.motors.size() == 0) {
+      84           0 :       return std::nullopt;
+      85             :     }
+      86             : 
+      87        5209 :     double throttle = 0;
+      88             : 
+      89       26045 :     for (size_t i = 0; i < msg.motors.size(); i++) {
+      90       20836 :       throttle += msg.motors[i];
+      91             :     };
+      92             : 
+      93        5209 :     throttle /= msg.motors.size();
+      94             : 
+      95        5209 :     return throttle;
+      96             :   }
+      97        5238 :   std::optional<double> operator()(const mrs_msgs::HwApiControlGroupCmd& msg) {
+      98        5238 :     return msg.throttle;
+      99             :   }
+     100       13432 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeCmd& msg) {
+     101       13432 :     return msg.throttle;
+     102             :   }
+     103       81397 :   std::optional<double> operator()(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+     104       81397 :     return msg.throttle;
+     105             :   }
+     106         997 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgRateCmd& msg) {
+     107         997 :     return std::nullopt;
+     108             :   }
+     109        1042 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiAccelerationHdgCmd& msg) {
+     110        1042 :     return std::nullopt;
+     111             :   }
+     112        2382 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+     113        2382 :     return std::nullopt;
+     114             :   }
+     115        1181 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+     116        1181 :     return std::nullopt;
+     117             :   }
+     118         477 :   std::optional<double> operator()([[maybe_unused]] const mrs_msgs::HwApiPositionCmd& msg) {
+     119         477 :     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        3816 :   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        3816 :     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        3816 :     return validateHwApiActuatorCmd(msg, node_name, var_name);
+     152             :   }
+     153        3845 :   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        3845 :     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        3845 :     return validateHwApiControlGroupCmd(msg, node_name, var_name);
+     162             :   }
+     163        7897 :   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        7897 :     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        7897 :     return validateHwApiAttitudeCmd(msg, node_name, var_name);
+     172             :   }
+     173       65096 :   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       65096 :     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       65096 :     return validateHwApiAttitudeRateCmd(msg, node_name, var_name);
+     182             :   }
+     183         997 :   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         997 :     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         997 :     return validateHwApiAccelerationHdgRateCmd(msg, node_name, var_name);
+     192             :   }
+     193        1042 :   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        1042 :     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        1042 :     return validateHwApiAccelerationHdgCmd(msg, node_name, var_name);
+     202             :   }
+     203        2253 :   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        2253 :     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        2253 :     return validateHwApiVelocityHdgRateCmd(msg, node_name, var_name);
+     212             :   }
+     213        1181 :   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        1181 :     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        1181 :     return validateHwApiVelocityHdgCmd(msg, node_name, var_name);
+     222             :   }
+     223         477 :   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         477 :     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         477 :     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       13719 :   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       13719 :     initializeHwApiCmd(msg, min_throttle);
+     267       13719 :   }
+     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..e04229abd45546c00d3c6ae8137138b531e3dcf1 GIT binary patch literal 934 zcmV;X16lluP)KzC z+s6y*mmp8nK% z-EV7y%-;I)I*gkAH1g<|172Pcym+|h$eiKCNn<^-&o$h5VfxVc4ygTF=8ujGs+L-P zN@dM3UarnFceWB3D`T$+fGh&+E(@|o1`RWUqZ+LPwbcQ(4M%A+b4@v|rRK&P>rv4$ zq1%>hEJY$ta`@9?ZQc*W0!=LQJWazyv^UT>m>2Q67zeKFx^3HQQ?{n<4V0O$0g!Cd zWYV1HiF&LfSwENjP{k&g5fkJ{o}KqwF)X#&RCp~F900L||t^=?@ z^__^;HQ8WWXgJZ@VcJ4vFWntVbu3M@D9tI?KOS_4XZDZ4gd5e0`j}KE(B=|laBXPH zyzZwArcDQBzOiN^X9dbUYv!(N=7!3gv}ShKrs2Fu8B86TGTR-Mv7@Rb>ZgowKLjpQ zCa!3|tW2QGPb(AGGpaK3-zWogxl9?X2l!oOB3+)|>>kRr^=7|!zo*9?pZtvi_c7(pihGfgFK(?h}ntV5YL54tTX--L~)0b9+CJE62YoBI*ginmMG5Qhe zw(wptqRaJbG3MS+@PPDPp?f>S88`dX(MD70vCli~1LfuGKNuljxyM8M5dZ)H07*qo IM6N<$g4TMu2mk;8 literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-f.html b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-f.html new file mode 100644 index 0000000000..ae67c42429 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/control_manager/index-detail-sort-f.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/control_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/control_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:539655.2 %
Date:2024-04-09 22:03:33Functions: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..b29554872e --- /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-04-09 22:03:33Functions: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..256d012fdd --- /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-04-09 22:03:33Functions: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..fddd631ced --- /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-04-09 22:03:33Functions: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..90bb306c59 --- /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-04-09 22:03:33Functions: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..98ef430635 --- /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-04-09 22:03:33Functions: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..f5f602c752 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/controller.h.func-sort-c.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/controller.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - controller.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-04-09 22:03:33Functions: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().2410
+
+
+ + + +
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..3a5105f3d0 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/controller.h.func.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/controller.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - controller.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-04-09 22:03:33Functions: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().2410
+
+
+ + + +
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..312d931e9a --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.html @@ -0,0 +1,232 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/controller.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - controller.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-04-09 22:03:33Functions: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         410 :   virtual ~Controller() = default;
+     144             : };
+     145             : 
+     146             : }  // namespace mrs_uav_managers
+     147             : 
+     148             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.overview.html b/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.overview.html new file mode 100644 index 0000000000..7ccc3a2839 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.overview.html @@ -0,0 +1,57 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/controller.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.png b/mrs_uav_managers/include/mrs_uav_managers/controller.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..3aa437cffc6a4bac8d6472ee5f8996438a7ffd35 GIT binary patch literal 681 zcmV;a0#^NrP)gyv~hF-^&dW?vH8lQFyr0yFtbG*21IvQ>PFm5wzv~q9s(U*D?$PkPb*mg9K z4;`1wwTa!aQn#n9$1^8R4EK&++_~G$Y7^}l1fj>V&qK*=9V{9pxp(yE8zD8n&L7kKa*6i+*$L2+n!s;Qt zWF?Y#<*tX-BVbX5&}fQ9abk54F_QX+6k<>0_iOxZEXoWOfTDC|i(szm@qstVFc_eX z((J0{GcAMJAVebuH?H!-vd*kO-exr{K^E;Rr%15l!zD`*xd6!R$#h<%{YC~}%o`2Y zF_4q^ZtTtxf79?l#N9r&5hJPP#F|~*gUnc*v;!8oqQ#i0K;)WWsvz1Pkm84qvG(+D zVkB92j8nwb(^YqYUagSMLXiJv9Z|i>lu*k8DdmHzvH*go2mfisL)0D + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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&)463
mrs_uav_managers::Estimator::~Estimator().2463
+
+
+ + + +
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..d7fa8a9775 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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&)463
mrs_uav_managers::Estimator::~Estimator()0
mrs_uav_managers::Estimator::~Estimator().2463
+
+
+ + + +
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..ef1b5472b1 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h.gcov.html @@ -0,0 +1,195 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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         463 :   Estimator(const std::string &type, const std::string &name, const std::string &frame_id) : type_(type), name_(name), frame_id_(frame_id) {
+      64         463 :   }
+      65             : 
+      66         463 :   virtual ~Estimator(void) {
+      67         463 :   }
+      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() const;
+      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..d24a7ae7fa04b7b1633468ba16c7ac71b827e7d9 GIT binary patch literal 505 zcmVHO=r)9+nepA@ zn&O{98B)Arlq6FOQk9AUg8ngfpv^wP-JvRqZctCy9yHThfLoJTHgz4ffeb)&g94s` z>s8=w;TR|)t5PE{+;gdBAlC@?yws^ERrjs#r5HE+=|XeT>L19ax#T;yC*#4z!I+9o zu4G82rk9Z}=q{iDgSN$EfniYhgjwK<>$(d7;yMzd*j%u+Q2y|?OlZFIBasa-nPBc_ zv)>C{3de`+Z0|qiMPJT#BOT}AvgE_QU~~I*HUh(4`?pky2sjp#LMYlHUH7LU4!RX% zK(|$~`Mj^9HC@eXL9P!MNJB8RUn&lY1c~tQ4Gxf^cfs{T_sj#dWGQwI*KU)E*9N#D z%oGF0R}|~^nC6+7Ol^`Akj+>Nxd2Dmd!OMZIgaKl- literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-f.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-f.html new file mode 100644 index 0000000000..eae84f367b --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10610997.2 %
Date:2024-04-09 22:03:33Functions: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..8dccae8963 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10610997.2 %
Date:2024-04-09 22:03:33Functions: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..4a8b3e207b --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10610997.2 %
Date:2024-04-09 22:03:33Functions: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..f9fa5b6dc5 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10610997.2 %
Date:2024-04-09 22:03:33Functions: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..2f0e36a01d --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10610997.2 %
Date:2024-04-09 22:03:33Functions: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..5be8de8bbf --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_managerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10610997.2 %
Date:2024-04-09 22:03:33Functions: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..341788464e --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func-sort-c.html @@ -0,0 +1,140 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - support.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10210597.1 %
Date:2024-04-09 22:03:33Functions: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&)4
mrs_uav_managers::estimation_manager::Support::frameIdToEstimatorName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)8
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&)256
mrs_uav_managers::estimation_manager::Support::toLowercase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)328
mrs_uav_managers::estimation_manager::Support::toSnakeCase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1833
mrs_uav_managers::estimation_manager::Support::applyPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)144616
mrs_uav_managers::estimation_manager::Support::rotateVecByHdg(geometry_msgs::Vector3_<std::allocator<void> > const&, double)248512
mrs_uav_managers::estimation_manager::Support::msgFromTf2(tf2::Transform const&)303456
mrs_uav_managers::estimation_manager::Support::uavStateToOdom(mrs_msgs::UavState_<std::allocator<void> > const&)313636
mrs_uav_managers::estimation_manager::Support::rotateVector(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)314593
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::Quaternion_<std::allocator<void> > const&)334477
mrs_uav_managers::estimation_manager::Support::pointToVector3(geometry_msgs::Point_<std::allocator<void> > const&)630582
mrs_uav_managers::estimation_manager::Support::poseFromTf2(tf2::Transform const&)645365
mrs_uav_managers::estimation_manager::Support::tf2FromPose(geometry_msgs::Pose_<std::allocator<void> > const&)949781
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::TransformStamped_<std::allocator<void> > const&)1090794
+
+
+ + + +
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..1677387fa6 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.func.html @@ -0,0 +1,140 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - support.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10210597.1 %
Date:2024-04-09 22:03:33Functions: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&)303456
mrs_uav_managers::estimation_manager::Support::getPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)4
mrs_uav_managers::estimation_manager::Support::poseFromTf2(tf2::Transform const&)645365
mrs_uav_managers::estimation_manager::Support::tf2FromPose(geometry_msgs::Pose_<std::allocator<void> > const&)949781
mrs_uav_managers::estimation_manager::Support::toLowercase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)328
mrs_uav_managers::estimation_manager::Support::toSnakeCase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)1833
mrs_uav_managers::estimation_manager::Support::rotateVector(geometry_msgs::Vector3_<std::allocator<void> > const&, geometry_msgs::Quaternion_<std::allocator<void> > const&)314593
mrs_uav_managers::estimation_manager::Support::applyPoseDiff(geometry_msgs::Pose_<std::allocator<void> > const&, geometry_msgs::Pose_<std::allocator<void> > const&)144616
mrs_uav_managers::estimation_manager::Support::pointToVector3(geometry_msgs::Point_<std::allocator<void> > const&)630582
mrs_uav_managers::estimation_manager::Support::rotateVecByHdg(geometry_msgs::Vector3_<std::allocator<void> > const&, double)248512
mrs_uav_managers::estimation_manager::Support::uavStateToOdom(mrs_msgs::UavState_<std::allocator<void> > const&)313636
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&)256
mrs_uav_managers::estimation_manager::Support::frameIdToEstimatorName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)8
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::Quaternion_<std::allocator<void> > const&)334477
mrs_uav_managers::estimation_manager::Support::noNans(geometry_msgs::TransformStamped_<std::allocator<void> > const&)1090794
+
+
+ + + +
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..43f24011fa --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.html @@ -0,0 +1,405 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers/estimation_manager - support.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:10210597.1 %
Date:2024-04-09 22:03:33Functions: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        1833 :   static std::string toSnakeCase(const std::string& str_in) {
+      39             : 
+      40        1833 :     std::string str(1, tolower(str_in[0]));
+      41             : 
+      42       28799 :     for (auto it = str_in.begin() + 1; it != str_in.end(); ++it) {
+      43       26966 :       if (isupper(*it) && *(it - 1) != '_' && islower(*(it - 1))) {
+      44        1515 :         str += "_";
+      45             :       }
+      46       26966 :       str += *it;
+      47             :     }
+      48             : 
+      49        1833 :     std::transform(str.begin(), str.end(), str.begin(), ::tolower);
+      50             : 
+      51        1833 :     return str;
+      52             :   }
+      53             :   /*//}*/
+      54             : 
+      55             : /* toLowercase() //{ */
+      56             : 
+      57         328 : static std::string toLowercase(const std::string str_in) {
+      58         328 :   std::string str_out = str_in;
+      59         328 :   std::transform(str_out.begin(), str_out.end(), str_out.begin(), ::tolower);
+      60         328 :   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      248512 :   static tf2::Vector3 rotateVecByHdg(const geometry_msgs::Vector3& vec_in, const double hdg_in) {
+      99             : 
+     100      248512 :     const tf2::Quaternion q_hdg = mrs_lib::AttitudeConverter(0, 0, 0).setHeading(hdg_in);
+     101             : 
+     102      245788 :     const tf2::Vector3 vec_tf2(vec_in.x, vec_in.y, vec_in.z);
+     103             : 
+     104      244705 :     const tf2::Vector3 vec_rotated = tf2::quatRotate(q_hdg, vec_tf2);
+     105             : 
+     106      488634 :     return vec_rotated;
+     107             :   }
+     108             :   //}
+     109             : 
+     110             :   /* noNans() //{ */
+     111     1090794 :   static bool noNans(const geometry_msgs::TransformStamped& tf) {
+     112             : 
+     113     3269474 :     return (std::isfinite(tf.transform.rotation.x) && std::isfinite(tf.transform.rotation.y) && std::isfinite(tf.transform.rotation.z) &&
+     114     4357785 :             std::isfinite(tf.transform.rotation.w) && std::isfinite(tf.transform.translation.x) && std::isfinite(tf.transform.translation.y) &&
+     115     2179650 :             std::isfinite(tf.transform.translation.z));
+     116             :   }
+     117             :   //}
+     118             : 
+     119             :   /* noNans() //{ */
+     120      334477 :   static bool noNans(const geometry_msgs::Quaternion& q) {
+     121             : 
+     122      334477 :     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      949781 :   static tf2::Transform tf2FromPose(const geometry_msgs::Pose& pose_in) {
+     136             : 
+     137      949781 :     tf2::Vector3 position(pose_in.position.x, pose_in.position.y, pose_in.position.z);
+     138             : 
+     139      949394 :     tf2::Quaternion q;
+     140      949329 :     tf2::fromMsg(pose_in.orientation, q);
+     141             : 
+     142      949244 :     tf2::Transform tf_out(q, position);
+     143             : 
+     144     1894690 :     return tf_out;
+     145             :   }
+     146             : 
+     147             :   //}
+     148             : 
+     149             :   /* poseFromTf2() //{ */
+     150             : 
+     151      645365 :   static geometry_msgs::Pose poseFromTf2(const tf2::Transform& tf_in) {
+     152             : 
+     153      645365 :     geometry_msgs::Pose pose_out;
+     154      644085 :     pose_out.position.x = tf_in.getOrigin().getX();
+     155      644808 :     pose_out.position.y = tf_in.getOrigin().getY();
+     156      644653 :     pose_out.position.z = tf_in.getOrigin().getZ();
+     157             : 
+     158      645632 :     pose_out.orientation = tf2::toMsg(tf_in.getRotation());
+     159             : 
+     160      645746 :     return pose_out;
+     161             :   }
+     162             : 
+     163             :   //}
+     164             : 
+     165             :   /* msgFromTf2() //{ */
+     166      303456 :   static geometry_msgs::Transform msgFromTf2(const tf2::Transform& tf_in) {
+     167             : 
+     168      303456 :     geometry_msgs::Transform tf_out;
+     169      303456 :     tf_out.translation.x = tf_in.getOrigin().getX();
+     170      303456 :     tf_out.translation.y = tf_in.getOrigin().getY();
+     171      303456 :     tf_out.translation.z = tf_in.getOrigin().getZ();
+     172      303456 :     tf_out.rotation = tf2::toMsg(tf_in.getRotation());
+     173             : 
+     174      303456 :     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      630582 :   static geometry_msgs::Vector3 pointToVector3(const geometry_msgs::Point& point_in) {
+     194             : 
+     195      630582 :     geometry_msgs::Vector3 vec_out;
+     196      630227 :     vec_out.x = point_in.x;
+     197      630227 :     vec_out.y = point_in.y;
+     198      630227 :     vec_out.z = point_in.z;
+     199             : 
+     200      630227 :     return vec_out;
+     201             :   }
+     202             : 
+     203             :   //}
+     204             : 
+     205             :   /*//{ rotateVector() */
+     206      314593 :   static geometry_msgs::Vector3 rotateVector(const geometry_msgs::Vector3& vec_in, const geometry_msgs::Quaternion& q_in) {
+     207             : 
+     208             :     try {
+     209      314593 :       Eigen::Matrix3d        R = mrs_lib::AttitudeConverter(q_in);
+     210      314610 :       Eigen::Vector3d        vec_in_eigen(vec_in.x, vec_in.y, vec_in.z);
+     211      314142 :       Eigen::Vector3d        vec_eigen_rotated = R * vec_in_eigen;
+     212      314406 :       geometry_msgs::Vector3 vec_out;
+     213      313553 :       vec_out.x = vec_eigen_rotated[0];
+     214      313502 :       vec_out.y = vec_eigen_rotated[1];
+     215      313804 :       vec_out.z = vec_eigen_rotated[2];
+     216      313944 :       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      313636 :   static nav_msgs::Odometry uavStateToOdom(const mrs_msgs::UavState& uav_state) {
+     227      313636 :     nav_msgs::Odometry odom;
+     228      313607 :     odom.header              = uav_state.header;
+     229      313571 :     odom.child_frame_id      = uav_state.child_frame_id;
+     230      313544 :     odom.pose.pose           = uav_state.pose;
+     231      313544 :     odom.twist.twist.angular = uav_state.velocity.angular;
+     232             : 
+     233      313544 :     tf2::Quaternion q;
+     234      313526 :     tf2::fromMsg(odom.pose.pose.orientation, q);
+     235      313511 :     odom.twist.twist.linear = Support::rotateVector(uav_state.velocity.linear, mrs_lib::AttitudeConverter(q.inverse()));
+     236             : 
+     237      625718 :     return odom;
+     238             :   }
+     239             :   /*//}*/
+     240             : 
+     241             :   /*//{ getPoseDiff() */
+     242           4 :   static geometry_msgs::Pose getPoseDiff(const geometry_msgs::Pose& p1, const geometry_msgs::Pose& p2) {
+     243             : 
+     244           4 :     tf2::Vector3 v1, v2;
+     245           4 :     tf2::fromMsg(p1.position, v1);
+     246           4 :     tf2::fromMsg(p2.position, v2);
+     247           4 :     const tf2::Vector3 v3 = v1 - v2;
+     248             : 
+     249           4 :     tf2::Quaternion q1, q2;
+     250           4 :     tf2::fromMsg(p1.orientation, q1);
+     251           4 :     tf2::fromMsg(p2.orientation, q2);
+     252           4 :     tf2::Quaternion q3 = q2 * q1.inverse();
+     253           4 :     q3.normalize();
+     254             : 
+     255           4 :     geometry_msgs::Pose pose_diff;
+     256           4 :     tf2::toMsg(v3, pose_diff.position);
+     257           4 :     pose_diff.orientation = tf2::toMsg(q3);
+     258             : 
+     259           8 :     return pose_diff;
+     260             :   }
+     261             :   /*//}*/
+     262             : 
+     263             :   /*//{ applyPoseDiff() */
+     264      144616 :   static geometry_msgs::Pose applyPoseDiff(const geometry_msgs::Pose& pose_in, const geometry_msgs::Pose& pose_diff) {
+     265             : 
+     266      144616 :     tf2::Vector3    pos_in;
+     267      144616 :     tf2::Quaternion q_in;
+     268      144616 :     tf2::fromMsg(pose_in.position, pos_in);
+     269      144616 :     tf2::fromMsg(pose_in.orientation, q_in);
+     270             : 
+     271      144616 :     tf2::Vector3    pos_diff;
+     272      144616 :     tf2::Quaternion q_diff;
+     273      144616 :     tf2::fromMsg(pose_diff.position, pos_diff);
+     274      144616 :     tf2::fromMsg(pose_diff.orientation, q_diff);
+     275             : 
+     276      144616 :     const tf2::Vector3    pos_out = tf2::quatRotate(q_diff.inverse(), (pos_in - pos_diff));
+     277      144616 :     const tf2::Quaternion q_out   = q_diff.inverse() * q_in;
+     278             : 
+     279      144616 :     geometry_msgs::Pose pose_out;
+     280      144616 :     tf2::toMsg(pos_out, pose_out.position);
+     281      144616 :     pose_out.orientation = tf2::toMsg(q_out);
+     282             : 
+     283      289232 :     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         256 :   static bool isStringInVector(const std::string& value, const std::vector<std::string>& str_vec) {
+     300         256 :     return std::find(str_vec.begin(), str_vec.end(), value) != str_vec.end();
+     301             :   }
+     302             :   /*//}*/
+     303             : 
+     304             :   /*//{ frameIdToEstimatorName() */
+     305           8 :   static std::string frameIdToEstimatorName(const std::string& str_in) {
+     306          16 :     const std::string str_tmp = str_in.substr(str_in.find("/")+1, str_in.size());
+     307          16 :     return str_tmp.substr(0, str_tmp.find("_origin") );
+     308             :   }
+     309             :   /*//}*/
+     310             : 
+     311             : private:
+     312             :   Support() {
+     313             :   }
+     314             : };
+     315             : /*//}*/
+     316             : 
+     317             : }  // namespace estimation_manager
+     318             : 
+     319             : }  // namespace mrs_uav_managers
+     320             : 
+     321             : #endif  // ESTIMATION_MANAGER_SUPPORT_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.overview.html b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.overview.html new file mode 100644 index 0000000000..4e128092e1 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.overview.html @@ -0,0 +1,101 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.png b/mrs_uav_managers/include/mrs_uav_managers/estimation_manager/support.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..600621ddc5631c697485f23bb4f61bac03ab2008 GIT binary patch literal 1232 zcmV;>1TXuEP)!aTS z5Vxy9aa$M9sV);`M%x_es-`opvt%zamd49~b&(2;7v*P+*T~+(SeKduCv-i{ynqp} zt`=~5&$R1S-*r3#i7w3&6ffc2>{^8=EOel}ywZVbG_g?7Js zOYN}r?4-J1fl5?sUnT#0=>^cV@jd;UHk^K}S4Vyd4Jhsu~gD~z$WSv=u z(-}ITB2b>r+!Hkg6^)ph9gQKV2U7M^>Y;6?<`EgM0NDh5xxzI|3r{K>X^&q1zIsx? z?q&eEyhkc4iX~{Wz5}I=Nc@!!MDD{sKgmTl0Y<>wzF*tJ58`ZT>%-MJqrFuKM4gpv z`9#;H;BubP5p{v$RVcTfud+wOV|(};Ld}UOBw@S*B#}kG0XQxUNl-ZJnW)eMevv(P z<15cSV0M_Hotv4~0&h&=`+z(HXUIS9&HC9{PB$O}A?e35UiwEgBH*#*5rlozDv;E( z2FKVR-{U)qQs`ND&UP+yDdlJ9;m2k|QdvV0s7cxVZ9rMeo}#<=@fDsTmu{QfRij|_ z6uBOqG$L`f?CtI4e#jP*%jQ5fS`FA5a0l)qW7~2Q5Qn2`dVLG9Z;W>uLWz zf7;CgEfU)E_2TcI1o+SXb{Tt}M&Pg3A5pMuQIj=Obfe)p4Ex8um4U$CaCnR;Jlyt5IPT=mbfITR!t)s$VjotR1Qf{h4W=N?}sNBD{Kl zrB$;+z)x6(on=^Ag=86;aVOAP9CUjDm#wdv}L(8>hN~JiX zO+lPfdvRfM4lya5f;ns~Zn=&_qR-9M!hF>RqWFJA{@Ie9&;jrrA2>9y!lBHRS;F@! zo(XZs4E(pQpZigPK$nIFln5+?CK;ao#J*5WVx2&8)ajD)?Jv6HRJ>?zV& zOtAUFmM=P*2B!e@rv#+7;DsZsSR^>Uhny6g-7DR|)k7)-r}wX?pMR9p{{ShfR7eLX uuF}o73AH3g65Djf>tl2z?7tTjweTN4ektG-6b(NB0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managersHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1212100.0 %
Date:2024-04-09 22:03:33Functions: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..213f26b01b --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index-detail-sort-l.html @@ -0,0 +1,164 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managersHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1212100.0 %
Date:2024-04-09 22:03:33Functions: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..a26e0f796e --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index-detail.html @@ -0,0 +1,164 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managersHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1212100.0 %
Date:2024-04-09 22:03:33Functions: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..e07ba051cf --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index-sort-f.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managersHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1212100.0 %
Date:2024-04-09 22:03:33Functions: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..af243bdddb --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index-sort-l.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managersHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1212100.0 %
Date:2024-04-09 22:03:33Functions: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..b3be280068 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/index.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managersHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1212100.0 %
Date:2024-04-09 22:03:33Functions: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..a6a44e6ced --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/state_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - state_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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&)86
mrs_uav_managers::StateEstimator::~StateEstimator().286
+
+
+ + + +
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..ea6cd3a68a --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/state_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - state_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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&)86
mrs_uav_managers::StateEstimator::~StateEstimator()0
mrs_uav_managers::StateEstimator::~StateEstimator().286
+
+
+ + + +
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..4260c4f8a4 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/state_estimator.h.gcov.html @@ -0,0 +1,169 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/state_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - state_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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          86 :   StateEstimator(const std::string &name, const std::string &frame_id, const std::string &package_name)
+      62          86 :       : Estimator(state::type, name, frame_id), package_name_(package_name) {
+      63          86 :   }
+      64             : 
+      65          86 :   virtual ~StateEstimator(void) {
+      66          86 :   }
+      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..82624f3756 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.func-sort-c.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/tracker.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - tracker.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-04-09 22:03:33Functions: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().2493
+
+
+ + + +
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..20d20720d6 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.func.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/tracker.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - tracker.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-04-09 22:03:33Functions: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().2493
+
+
+ + + +
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..915348b515 --- /dev/null +++ b/mrs_uav_managers/include/mrs_uav_managers/tracker.h.gcov.html @@ -0,0 +1,296 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/mrs_uav_managers/tracker.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/mrs_uav_managers - tracker.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:11100.0 %
Date:2024-04-09 22:03:33Functions: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         493 :   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:19139848.0 %
Date:2024-04-09 22:03:33Functions: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.6%79.6%
+ 		79.6 %191 / 24092.9 %13 / 14
<unnamed>79.6 %191 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index-detail-sort-l.html b/mrs_uav_managers/include/transform_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..b905c7244e --- /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:19139848.0 %
Date:2024-04-09 22:03:33Functions: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.6%79.6%
+ 		79.6 %191 / 24092.9 %13 / 14
<unnamed>79.6 %191 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index-detail.html b/mrs_uav_managers/include/transform_manager/index-detail.html new file mode 100644 index 0000000000..b1e3b0f9f8 --- /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:19139848.0 %
Date:2024-04-09 22:03:33Functions: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.6%79.6%
+ 		79.6 %191 / 24092.9 %13 / 14
<unnamed>79.6 %191 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index-sort-f.html b/mrs_uav_managers/include/transform_manager/index-sort-f.html new file mode 100644 index 0000000000..59b4c46d7f --- /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:19139848.0 %
Date:2024-04-09 22:03:33Functions: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.6%79.6%
+ 		79.6 %191 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index-sort-l.html b/mrs_uav_managers/include/transform_manager/index-sort-l.html new file mode 100644 index 0000000000..4a6e7da739 --- /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:19139848.0 %
Date:2024-04-09 22:03:33Functions: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.6%79.6%
+ 		79.6 %191 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/index.html b/mrs_uav_managers/include/transform_manager/index.html new file mode 100644 index 0000000000..dd0611ba05 --- /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:19139848.0 %
Date:2024-04-09 22:03:33Functions: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.6%79.6%
+ 		79.6 %191 / 24092.9 %13 / 14
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func-sort-c.html b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func-sort-c.html new file mode 100644 index 0000000000..75892a8b06 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func-sort-c.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_mapping_origin.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_mapping_origin.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:01580.0 %
Date:2024-04-09 22:03:33Functions: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..321ccae6e5 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.func.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_mapping_origin.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_mapping_origin.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:01580.0 %
Date:2024-04-09 22:03:33Functions: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..f6090a8bb9 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_mapping_origin.h.gcov.html @@ -0,0 +1,474 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_mapping_origin.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_mapping_origin.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:01580.0 %
Date:2024-04-09 22:03:33Functions: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:19124079.6 %
Date:2024-04-09 22:03:33Functions: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)8
mrs_uav_managers::TfSource::getIsUtmSource()16
mrs_uav_managers::TfSource::getIsUtmBased()17
mrs_uav_managers::TfSource::setUtmOrigin(geometry_msgs::Point_<std::allocator<void> > const&)86
mrs_uav_managers::TfSource::setWorldOrigin(geometry_msgs::Point_<std::allocator<void> > const&)86
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)86
mrs_uav_managers::TfSource::getName[abi:cxx11]()704
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> > >&)132803
mrs_uav_managers::TfSource::publishTfFromOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)167762
mrs_uav_managers::TfSource::callbackTfSourceOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)167844
mrs_uav_managers::TfSource::publishTfFromAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)173427
mrs_uav_managers::TfSource::callbackTfSourceAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)173484
mrs_uav_managers::TfSource::getPrintName[abi:cxx11]()812574
+
+
+ + + +
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..28edcec012 --- /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:19124079.6 %
Date:2024-04-09 22:03:33Functions: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]()812574
mrs_uav_managers::TfSource::setUtmOrigin(geometry_msgs::Point_<std::allocator<void> > const&)86
mrs_uav_managers::TfSource::getIsUtmBased()17
mrs_uav_managers::TfSource::getIsUtmSource()16
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)8
mrs_uav_managers::TfSource::setWorldOrigin(geometry_msgs::Point_<std::allocator<void> > const&)86
mrs_uav_managers::TfSource::publishTfFromAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)173427
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> > >&)132803
mrs_uav_managers::TfSource::publishTfFromOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)167762
mrs_uav_managers::TfSource::callbackTfSourceAtt(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)173484
mrs_uav_managers::TfSource::callbackTfSourceOdom(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)167844
mrs_uav_managers::TfSource::getName[abi:cxx11]()704
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)86
+
+
+ + + +
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..3c53bf1091 --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.html @@ -0,0 +1,704 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_source.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/include/transform_manager - tf_source.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19124079.6 %
Date:2024-04-09 22:03:33Functions: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          86 :   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          86 :       : name_(name), nh_(nh), broadcaster_(broadcaster), ch_(ch), is_utm_source_(is_utm_source) {
+      42             : 
+      43          86 :     ROS_INFO("[%s]: initializing", getPrintName().c_str());
+      44             : 
+      45          86 :     if (name != "dummy") {
+      46             : 
+      47             :       /*//{ load parameters */
+      48             : 
+      49         172 :       const std::string yaml_prefix = "mrs_uav_managers/transform_manager/";
+      50             : 
+      51         172 :       std::string odom_topic, attitude_topic, ns;
+      52             : 
+      53          86 :       param_loader->loadParam(yaml_prefix + getName() + "/odom_topic", odom_topic);
+      54          86 :       param_loader->loadParam(yaml_prefix + getName() + "/custom_frame_id/enabled", custom_frame_id_enabled_, false);
+      55          86 :       if (custom_frame_id_enabled_) {
+      56           0 :         param_loader->loadParam(yaml_prefix + getName() + "/custom_frame_id/frame_id", custom_frame_id_);
+      57             :       }
+      58          86 :       param_loader->loadParam(yaml_prefix + getName() + "/tf_from_attitude/enabled", tf_from_attitude_enabled_);
+      59          86 :       if (tf_from_attitude_enabled_) {
+      60          85 :         param_loader->loadParam(yaml_prefix + getName() + "/tf_from_attitude/attitude_topic", attitude_topic);
+      61             :       }
+      62          86 :       param_loader->loadParam(yaml_prefix + getName() + "/namespace", ns);
+      63          86 :       full_topic_odom_     = "/" + ch_->uav_name + "/" + ns + "/" + odom_topic;
+      64          86 :       full_topic_attitude_ = "/" + ch_->uav_name + "/" + ns + "/" + attitude_topic;
+      65          86 :       param_loader->loadParam(yaml_prefix + getName() + "/inverted", is_inverted_);
+      66          86 :       param_loader->loadParam(yaml_prefix + getName() + "/republish_in_frames", republish_in_frames_);
+      67             : 
+      68             :       /* coordinate frames origins //{ */
+      69          86 :       param_loader->loadParam(yaml_prefix + getName() + "/utm_based", is_utm_based_);
+      70             :       /* param_loader->loadParam(yaml_prefix + getName() + "/publish_local_tf", publish_local_tf_); */
+      71             : 
+      72             :       /*//{ utm source */
+      73          86 :       if (is_utm_based_) {
+      74         170 :         std::string utm_origin_parent_frame_id;
+      75          85 :         param_loader->loadParam(yaml_prefix + "utm_origin_tf/parent", utm_origin_parent_frame_id);
+      76          85 :         ns_utm_origin_parent_frame_id_ = ch_->uav_name + "/" + utm_origin_parent_frame_id;
+      77             : 
+      78          85 :         std::string utm_origin_child_frame_id;
+      79          85 :         param_loader->loadParam(yaml_prefix + "utm_origin_tf/child", utm_origin_child_frame_id);
+      80          85 :         ns_utm_origin_child_frame_id_ = ch_->uav_name + "/" + utm_origin_child_frame_id;
+      81             :       }
+      82             :       /*//}*/
+      83             : 
+      84             :       /*//{ world source */
+      85          86 :       if (is_utm_based_) {
+      86         170 :         std::string world_origin_parent_frame_id;
+      87          85 :         param_loader->loadParam(yaml_prefix + "world_origin_tf/parent", world_origin_parent_frame_id);
+      88          85 :         ns_world_origin_parent_frame_id_ = ch_->uav_name + "/" + world_origin_parent_frame_id;
+      89             : 
+      90          85 :         std::string world_origin_child_frame_id;
+      91          85 :         param_loader->loadParam(yaml_prefix + "world_origin_tf/child", world_origin_child_frame_id);
+      92          85 :         ns_world_origin_child_frame_id_ = ch_->uav_name + "/" + world_origin_child_frame_id;
+      93             :       }
+      94             :       /*//}*/
+      95             : 
+      96             :       //}
+      97             : 
+      98          86 :       if (!param_loader->loadedSuccessfully()) {
+      99           0 :         ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str());
+     100           0 :         ros::shutdown();
+     101             :       }
+     102             : 
+     103             :       /*//}*/
+     104             : 
+     105             :       /*//{ initialize subscribers */
+     106         172 :       mrs_lib::SubscribeHandlerOptions shopts;
+     107          86 :       shopts.nh                 = nh_;
+     108          86 :       shopts.node_name          = getPrintName();
+     109          86 :       shopts.no_message_timeout = mrs_lib::no_timeout;
+     110          86 :       shopts.threadsafe         = true;
+     111          86 :       shopts.autostart          = true;
+     112          86 :       shopts.queue_size         = 10;
+     113          86 :       shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     114             : 
+     115          86 :       sh_tf_source_odom_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, full_topic_odom_, &TfSource::callbackTfSourceOdom, this);
+     116          86 :       if (tf_from_attitude_enabled_) {
+     117          85 :         sh_tf_source_att_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, full_topic_attitude_, &TfSource::callbackTfSourceAtt, this);
+     118             :       }
+     119             : 
+     120          86 :       if (is_utm_source_) {
+     121          81 :         sh_altitude_amsl_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in");
+     122             :       }
+     123             : 
+     124             :     }
+     125             :     /*//}*/
+     126             : 
+     127         152 :     for (auto frame_id : republish_in_frames_) {
+     128          66 :       republishers_.push_back(
+     129         132 :           std::make_pair(frame_id, mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, full_topic_odom_ + "/" + frame_id.substr(0, frame_id.find("_origin")))));
+     130             :     }
+     131          86 :     is_initialized_ = true;
+     132          86 :     ROS_INFO("[%s]: initialized", getPrintName().c_str());
+     133          86 :   }
+     134             :   /*//}*/
+     135             : 
+     136             :   /*//{ getName() */
+     137         704 :   std::string getName() {
+     138         704 :     return name_;
+     139             :   }
+     140             :   /*//}*/
+     141             : 
+     142             :   /*//{ getPrintName() */
+     143      812574 :   std::string getPrintName() {
+     144     1627708 :     return ch_->nodelet_name + "/" + name_;
+     145             :   }
+     146             :   /*//}*/
+     147             : 
+     148             :   /*//{ getIsUtmBased() */
+     149          17 :   bool getIsUtmBased() {
+     150          17 :     return is_utm_based_;
+     151             :   }
+     152             :   /*//}*/
+     153             : 
+     154             :   /*//{ getIsUtmSource() */
+     155          16 :   bool getIsUtmSource() {
+     156          16 :     return is_utm_source_;
+     157             :   }
+     158             :   /*//}*/
+     159             : 
+     160             :   /*//{ setIsUtmSource() */
+     161           8 :   void setIsUtmSource(const bool is_utm_source) {
+     162           8 :     if (is_utm_source) {
+     163           4 :       mrs_lib::SubscribeHandlerOptions shopts;
+     164           4 :       shopts.nh                 = nh_;
+     165           4 :       shopts.node_name          = getPrintName();
+     166           4 :       shopts.no_message_timeout = mrs_lib::no_timeout;
+     167           4 :       shopts.threadsafe         = true;
+     168           4 :       shopts.autostart          = true;
+     169           4 :       shopts.queue_size         = 10;
+     170           4 :       shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     171           4 :       sh_altitude_amsl_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in");
+     172             :     }
+     173           8 :     is_utm_source_ = is_utm_source;
+     174           8 :   }
+     175             :   /*//}*/
+     176             : 
+     177             :   /*//{ setUtmOrigin() */
+     178          86 :   void setUtmOrigin(const geometry_msgs::Point& pt) {
+     179             : 
+     180          86 :     if (is_utm_based_ && !is_utm_origin_set_) {
+     181          85 :       utm_origin_        = pt;
+     182          85 :       is_utm_origin_set_ = true;
+     183             :     }
+     184          86 :   }
+     185             :   /*//}*/
+     186             : 
+     187             :   /*//{ setWorldOrigin() */
+     188          86 :   void setWorldOrigin(const geometry_msgs::Point& pt) {
+     189             : 
+     190          86 :     if (is_utm_based_ && !is_world_origin_set_) {
+     191          85 :       world_origin_        = pt;
+     192          85 :       is_world_origin_set_ = true;
+     193             :     }
+     194          86 :   }
+     195             :   /*//}*/
+     196             : 
+     197             : private:
+     198             :   const std::string name_;
+     199             :   const std::string ns_fcu_frame_id_;
+     200             : 
+     201             :   ros::NodeHandle nh_;
+     202             : 
+     203             :   std::shared_ptr<mrs_lib::TransformBroadcaster> broadcaster_;
+     204             :   tf2_ros::StaticTransformBroadcaster            static_broadcaster_;
+     205             : 
+     206             :   std::shared_ptr<estimation_manager::CommonHandlers_t> ch_;
+     207             : 
+     208             :   bool is_inverted_;
+     209             : 
+     210             :   bool             is_utm_based_;
+     211             :   bool             publish_local_tf_ = false;
+     212             :   bool             publish_utm_tf_   = false;
+     213             :   bool             publish_world_tf_ = false;
+     214             :   std::atomic_bool is_utm_source_    = false;
+     215             :   std::string      ns_utm_origin_parent_frame_id_;
+     216             :   std::string      ns_utm_origin_child_frame_id_;
+     217             : 
+     218             :   bool                 is_utm_origin_set_ = false;
+     219             :   geometry_msgs::Point utm_origin_;
+     220             : 
+     221             :   std::string ns_world_origin_parent_frame_id_;
+     222             :   std::string ns_world_origin_child_frame_id_;
+     223             : 
+     224             :   bool                 is_world_origin_set_ = false;
+     225             :   geometry_msgs::Point world_origin_;
+     226             : 
+     227             :   std::string full_topic_odom_;
+     228             :   std::string full_topic_attitude_;
+     229             :   bool        tf_from_attitude_enabled_ = false;
+     230             : 
+     231             :   bool        custom_frame_id_enabled_;
+     232             :   std::string custom_frame_id_;
+     233             : 
+     234             :   std::atomic_bool is_initialized_               = false;
+     235             :   std::atomic_bool is_local_static_tf_published_ = false;
+     236             :   std::atomic_bool is_utm_static_tf_published_   = false;
+     237             :   std::atomic_bool is_world_static_tf_published_ = false;
+     238             : 
+     239             :   std::vector<std::string> republish_in_frames_;
+     240             : 
+     241             :   std::vector<std::pair<std::string, mrs_lib::PublisherHandler<nav_msgs::Odometry>>> republishers_;
+     242             : 
+     243             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry>               sh_tf_source_odom_;
+     244             :   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_tf_source_att_;
+     245             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>          sh_altitude_amsl_;
+     246             :   nav_msgs::OdometryConstPtr                                  first_msg_;
+     247             :   bool                                                        got_first_msg_ = false;
+     248             : 
+     249             : 
+     250             :   /*//{ callbackTfSourceOdom()*/
+     251      167844 :   void callbackTfSourceOdom(const nav_msgs::Odometry::ConstPtr msg) {
+     252             : 
+     253      167844 :     if (!is_initialized_) {
+     254           0 :       return;
+     255             :     }
+     256             : 
+     257      503537 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::callbackTfSourceOdom", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     258             : 
+     259      167797 :     scope_timer.checkpoint("get msg");
+     260             : 
+     261      167855 :     if (!got_first_msg_) {
+     262          86 :       first_msg_     = msg;
+     263          86 :       got_first_msg_ = true;
+     264             :     }
+     265             : 
+     266      167855 :     publishTfFromOdom(msg);
+     267      167876 :     scope_timer.checkpoint("pub tf");
+     268             : 
+     269      167874 :     if (publish_local_tf_ && !is_local_static_tf_published_) {
+     270           0 :       publishLocalTf(msg->header.frame_id);
+     271           0 :       scope_timer.checkpoint("pub local tf");
+     272             :     }
+     273             : 
+     274             :     /* if (publish_utm_tf_ && is_utm_based_ && is_utm_origin_set_ && !is_utm_static_tf_published_) { */
+     275             :     /*   publishUtmTf(msg->header.frame_id); */
+     276             :     /*   scope_timer.checkpoint("pub utm tf"); */
+     277             :     /* } */
+     278             : 
+     279             :     /* if (publish_world_tf_ && is_utm_based_ && is_world_origin_set_ && !is_world_static_tf_published_) { */
+     280             :     /*   publishWorldTf(msg->header.frame_id); */
+     281             :     /*   scope_timer.checkpoint("pub world tf"); */
+     282             :     /* } */
+     283             : 
+     284      300677 :     for (auto republisher : republishers_) {
+     285      132803 :       republishInFrame(msg, ch_->uav_name + "/" + republisher.first, republisher.second);
+     286             :     }
+     287             :   }
+     288             :   /*//}*/
+     289             : 
+     290             :   /*//{ callbackTfSourceAtt()*/
+     291      173484 :   void callbackTfSourceAtt(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     292             : 
+     293      173484 :     if (!is_initialized_) {
+     294           0 :       return;
+     295             :     }
+     296             : 
+     297      520416 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::callbackTfSourceAtt", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     298             : 
+     299      173444 :     scope_timer.checkpoint("get msg");
+     300      173477 :     publishTfFromAtt(msg);
+     301             :   }
+     302             :   /*//}*/
+     303             : 
+     304             :   /* publishTfFromOdom() //{*/
+     305      167762 :   void publishTfFromOdom(const nav_msgs::OdometryConstPtr& odom) {
+     306             : 
+     307      335622 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishTfFromOdom", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     308             : 
+     309      167851 :     const tf2::Transform      tf       = Support::tf2FromPose(odom->pose.pose);
+     310      167812 :     const tf2::Transform      tf_inv   = tf.inverse();
+     311      167819 :     const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     312             : 
+     313             :     /*//{ tf source origin */
+     314      167810 :     geometry_msgs::TransformStamped tf_msg;
+     315      167744 :     tf_msg.header.stamp         = odom->header.stamp;
+     316      167758 :     std::string origin_frame_id = custom_frame_id_enabled_ ? ch_->uav_name + "/" + custom_frame_id_ : odom->header.frame_id;
+     317      167765 :     if (is_inverted_) {
+     318             : 
+     319      167765 :       tf_msg.header.frame_id       = ch_->frames.ns_fcu;
+     320      167789 :       tf_msg.child_frame_id        = origin_frame_id;
+     321      167765 :       tf_msg.transform.translation = Support::pointToVector3(pose_inv.position);
+     322      167837 :       tf_msg.transform.rotation    = pose_inv.orientation;
+     323             : 
+     324             :     } else {
+     325           0 :       tf_msg.header.frame_id       = origin_frame_id;
+     326           0 :       tf_msg.child_frame_id        = ch_->frames.ns_fcu;
+     327           0 :       tf_msg.transform.translation = Support::pointToVector3(odom->pose.pose.position);
+     328           0 :       tf_msg.transform.rotation    = odom->pose.pose.orientation;
+     329             :     }
+     330             : 
+     331      167837 :     if (Support::noNans(tf_msg)) {
+     332             :       try {
+     333      167806 :         broadcaster_->sendTransform(tf_msg);
+     334             :       }
+     335           0 :       catch (...) {
+     336           0 :         ROS_ERROR("exception caught ");
+     337             :       }
+     338             : 
+     339      167878 :       if (tf_from_attitude_enabled_) {
+     340      166821 :         if (is_inverted_) {
+     341      166821 :           tf_msg.child_frame_id += "_att_only";
+     342             :         } else {
+     343           0 :           tf_msg.header.frame_id += "_att_only";
+     344             :         }
+     345             :         try {
+     346      166821 :           broadcaster_->sendTransform(tf_msg);
+     347             :         }
+     348           0 :         catch (...) {
+     349           0 :           ROS_ERROR("exception caught ");
+     350             :         }
+     351             :       }
+     352             :       /*//}*/
+     353             : 
+     354             :       /*//{ tf utm origin */
+     355             : 
+     356      167878 :       geometry_msgs::TransformStamped tf_utm_msg;
+     357      167878 :       if (is_utm_source_) {
+     358             :         
+     359      151728 :         if (!is_utm_origin_set_) {
+     360           0 :           ROS_INFO_THROTTLE(5.0, "[%s]: %s utm_origin initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     361           0 :           return;
+     362             :         }
+     363             : 
+     364      151728 :         geometry_msgs::Pose pose_utm = odom->pose.pose;
+     365      151728 :         pose_utm.position.x += utm_origin_.x - first_msg_->pose.pose.position.x;
+     366      151728 :         pose_utm.position.y += utm_origin_.y - first_msg_->pose.pose.position.y;
+     367             : 
+     368      151728 :         if (sh_altitude_amsl_.hasMsg()) {
+     369      151728 :           pose_utm.position.z = sh_altitude_amsl_.getMsg()->amsl;
+     370             :         } else {
+     371           0 :           ROS_WARN_THROTTLE(5.0, "[%s]: AMSL altitude not available.", getPrintName().c_str());
+     372             :         }
+     373             : 
+     374      151728 :         tf_utm_msg.header.stamp    = odom->header.stamp;
+     375      151728 :         tf_utm_msg.header.frame_id = ns_utm_origin_parent_frame_id_;
+     376      151728 :         tf_utm_msg.child_frame_id  = ns_utm_origin_child_frame_id_;
+     377             : 
+     378      151728 :         tf2::Transform tf_utm;
+     379      151728 :         if (is_inverted_) {
+     380      151728 :           tf_utm = Support::tf2FromPose(pose_utm).inverse();
+     381             :         } else {
+     382           0 :           tf_utm = Support::tf2FromPose(pose_utm);
+     383             :         }
+     384      151728 :         tf_utm_msg.transform = Support::msgFromTf2(tf_utm);
+     385             : 
+     386             :         try {
+     387      151728 :           broadcaster_->sendTransform(tf_utm_msg);
+     388      151728 :           ROS_INFO_ONCE("[%s]: publishing utm_origin tf", getPrintName().c_str());
+     389             :         }
+     390           0 :         catch (...) {
+     391           0 :           ROS_ERROR("exception caught ");
+     392             :         }
+     393             :       }
+     394             :       /*//}*/
+     395             : 
+     396             :       /*//{ tf world origin */
+     397      167876 :       if (is_utm_source_) {
+     398      303456 :         geometry_msgs::TransformStamped tf_world_msg;
+     399      151728 :         tf_world_msg.header.stamp    = odom->header.stamp;
+     400      151728 :         tf_world_msg.header.frame_id = ns_world_origin_parent_frame_id_;
+     401      151728 :         tf_world_msg.child_frame_id  = ns_world_origin_child_frame_id_;
+     402             : 
+     403      151728 :         tf2::Transform tf_world;
+     404      151728 :         tf_world.setOrigin(tf2::Vector3(world_origin_.x, world_origin_.y, world_origin_.z));
+     405      151728 :         tf_world.setRotation(tf2::Quaternion(0, 0, 0, 1));
+     406             : 
+     407      151728 :         geometry_msgs::Pose pose_utm = odom->pose.pose;
+     408      151728 :         pose_utm.position.x += utm_origin_.x - first_msg_->pose.pose.position.x;
+     409      151728 :         pose_utm.position.y += utm_origin_.y - first_msg_->pose.pose.position.y;
+     410             : 
+     411      151728 :         tf2::Transform tf_utm;
+     412      151728 :         if (is_inverted_) {
+     413      151728 :           tf_utm = Support::tf2FromPose(pose_utm).inverse();
+     414             :         } else {
+     415           0 :           tf_utm = Support::tf2FromPose(pose_utm);
+     416             :         }
+     417             : 
+     418      151728 :         tf_world_msg.transform          = Support::msgFromTf2(tf_utm * tf_world);
+     419      151728 :         tf_world_msg.transform.rotation = pose_inv.orientation;
+     420             : 
+     421             :         try {
+     422      151728 :           broadcaster_->sendTransform(tf_world_msg);
+     423      151728 :           ROS_INFO_ONCE("[%s]: publishing world_origin tf", getPrintName().c_str());
+     424             :         }
+     425           0 :         catch (...) {
+     426           0 :           ROS_ERROR("exception caught ");
+     427             :         }
+     428             :       }
+     429             : 
+     430             :       /*//}*/
+     431             : 
+     432             :     } else {
+     433           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     434             :                         tf_msg.child_frame_id.c_str());
+     435             :     }
+     436      167877 :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on topic: %s", getPrintName().c_str(),
+     437             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_odom_.c_str());
+     438             :   }
+     439             :   /*//}*/
+     440             : 
+     441             :   /* publishTfFromAtt() //{*/
+     442      173427 :   void publishTfFromAtt(const geometry_msgs::QuaternionStampedConstPtr& msg) {
+     443             : 
+     444      520400 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishTfFromAtt", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     445             : 
+     446      346967 :     geometry_msgs::TransformStamped tf_msg;
+     447      173461 :     tf_msg.header.stamp = msg->header.stamp;
+     448             : 
+     449      173437 :     geometry_msgs::Pose pose;
+     450      173433 :     pose.position.x = 0.0;
+     451      173433 :     pose.position.y = 0.0;
+     452      173433 :     pose.position.z = 0.0;
+     453      173433 :     pose.orientation = msg->quaternion;
+     454      173433 :     if (is_inverted_) {
+     455             : 
+     456      173433 :       const tf2::Transform      tf       = Support::tf2FromPose(pose);
+     457      173460 :       const tf2::Transform      tf_inv   = tf.inverse();
+     458      173440 :       const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     459             : 
+     460      173435 :       tf_msg.header.frame_id       = ch_->frames.ns_fcu;
+     461      173443 :       tf_msg.child_frame_id        = msg->header.frame_id;
+     462      173438 :       tf_msg.transform.translation = Support::pointToVector3(pose_inv.position);
+     463      173470 :       tf_msg.transform.rotation    = pose_inv.orientation;
+     464             : 
+     465             :     } else {
+     466           0 :       tf_msg.header.frame_id       = msg->header.frame_id;
+     467           0 :       tf_msg.child_frame_id        = ch_->frames.ns_fcu;
+     468           0 :       tf_msg.transform.translation = Support::pointToVector3(pose.position);
+     469           0 :       tf_msg.transform.rotation    = pose.orientation;
+     470             :     }
+     471             : 
+     472      173470 :     if (Support::noNans(tf_msg)) {
+     473             :       try {
+     474      173435 :         scope_timer.checkpoint("before pub");
+     475      173450 :         broadcaster_->sendTransform(tf_msg);
+     476      173493 :         scope_timer.checkpoint("after pub");
+     477             :       }
+     478           0 :       catch (...) {
+     479           0 :         ROS_ERROR("exception caught ");
+     480             :       }
+     481             :     } else {
+     482           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     483             :                         tf_msg.child_frame_id.c_str());
+     484             :     }
+     485      173489 :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on topic: %s", getPrintName().c_str(),
+     486             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_attitude_.c_str());
+     487      173492 :   }
+     488             :   /*//}*/
+     489             : 
+     490             :   /* publishLocalTf() //{*/
+     491           0 :   void publishLocalTf(const std::string& frame_id) {
+     492             : 
+     493           0 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishLocalTf", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     494             : 
+     495           0 :     geometry_msgs::TransformStamped tf_msg;
+     496           0 :     tf_msg.header.stamp = ros::Time::now();
+     497             : 
+     498           0 :     tf_msg.header.frame_id       = frame_id;
+     499           0 :     tf_msg.child_frame_id        = frame_id.substr(0, frame_id.find("_origin")) + "_local_origin";
+     500           0 :     tf_msg.transform.translation = Support::pointToVector3(first_msg_->pose.pose.position);
+     501           0 :     tf_msg.transform.rotation    = first_msg_->pose.pose.orientation;
+     502             : 
+     503           0 :     if (Support::noNans(tf_msg)) {
+     504             :       try {
+     505           0 :         static_broadcaster_.sendTransform(tf_msg);
+     506             :       }
+     507           0 :       catch (...) {
+     508           0 :         ROS_ERROR("exception caught ");
+     509             :       }
+     510             :     } else {
+     511           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     512             :                         tf_msg.child_frame_id.c_str());
+     513             :     }
+     514           0 :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on first message of: %s", getPrintName().c_str(),
+     515             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_odom_.c_str());
+     516           0 :     is_local_static_tf_published_ = true;
+     517           0 :   }
+     518             :   /*//}*/
+     519             : 
+     520             :   /* publishUtmTf() //{*/
+     521             :   void publishUtmTf(const std::string& frame_id) {
+     522             : 
+     523             :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishUtmTf", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     524             : 
+     525             :     geometry_msgs::TransformStamped tf_msg;
+     526             :     tf_msg.header.stamp = ros::Time::now();
+     527             : 
+     528             :     tf_msg.header.frame_id         = frame_id;
+     529             :     tf_msg.child_frame_id          = frame_id.substr(0, frame_id.find("_origin")) + "_utm_origin";
+     530             :     tf_msg.transform.translation.x = -utm_origin_.x;  // minus because inverse tf tree
+     531             :     tf_msg.transform.translation.y = -utm_origin_.y;  // minus because inverse tf tree
+     532             :     tf_msg.transform.translation.z = -utm_origin_.z;  // minus because inverse tf tree
+     533             :     tf_msg.transform.rotation.x    = 0;
+     534             :     tf_msg.transform.rotation.y    = 0;
+     535             :     tf_msg.transform.rotation.z    = 0;
+     536             :     tf_msg.transform.rotation.w    = 1;
+     537             : 
+     538             :     if (Support::noNans(tf_msg)) {
+     539             :       try {
+     540             :         static_broadcaster_.sendTransform(tf_msg);
+     541             :       }
+     542             :       catch (...) {
+     543             :         ROS_ERROR("exception caught ");
+     544             :       }
+     545             :     } else {
+     546             :       ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     547             :                         tf_msg.child_frame_id.c_str());
+     548             :     }
+     549             :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on first message of: %s", getPrintName().c_str(),
+     550             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_odom_.c_str());
+     551             :     is_utm_static_tf_published_ = true;
+     552             :   }
+     553             :   /*//}*/
+     554             : 
+     555             :   /* publishWorldTf() //{*/
+     556             :   void publishWorldTf(const std::string& frame_id) {
+     557             : 
+     558             :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishWorldTf", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     559             : 
+     560             :     geometry_msgs::TransformStamped tf_msg;
+     561             :     tf_msg.header.stamp = ros::Time::now();
+     562             : 
+     563             :     tf_msg.header.frame_id         = frame_id;
+     564             :     tf_msg.child_frame_id          = frame_id.substr(0, frame_id.find("_origin")) + "_world_origin";
+     565             :     tf_msg.transform.translation.x = -(utm_origin_.x - world_origin_.x);  // minus because inverse tf tree
+     566             :     tf_msg.transform.translation.y = -(utm_origin_.y - world_origin_.y);  // minus because inverse tf tree
+     567             :     /* tf_msg.transform.translation.z = -(utm_origin_.z);                    // minus because inverse tf tree */
+     568             :     tf_msg.transform.translation.z = 0;
+     569             :     tf_msg.transform.rotation.x    = 0;
+     570             :     tf_msg.transform.rotation.y    = 0;
+     571             :     tf_msg.transform.rotation.z    = 0;
+     572             :     tf_msg.transform.rotation.w    = 1;
+     573             : 
+     574             :     if (Support::noNans(tf_msg)) {
+     575             :       try {
+     576             :         static_broadcaster_.sendTransform(tf_msg);
+     577             :       }
+     578             :       catch (...) {
+     579             :         ROS_ERROR("exception caught ");
+     580             :       }
+     581             :     } else {
+     582             :       ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     583             :                         tf_msg.child_frame_id.c_str());
+     584             :     }
+     585             :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s based on first message of: %s", getPrintName().c_str(),
+     586             :                   tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str(), full_topic_odom_.c_str());
+     587             :     is_world_static_tf_published_ = true;
+     588             :   }
+     589             :   /*//}*/
+     590             : 
+     591             :   /* republishInFrame() //{*/
+     592      132803 :   void republishInFrame(const nav_msgs::OdometryConstPtr& msg, const std::string& frame_id, mrs_lib::PublisherHandler<nav_msgs::Odometry>& ph) {
+     593             : 
+     594      265606 :     mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::republishInFrame", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     595             : 
+     596      132803 :     nav_msgs::Odometry msg_out = *msg;
+     597      132803 :     msg_out.header.frame_id    = frame_id;
+     598             : 
+     599      132803 :     geometry_msgs::PoseStamped pose;
+     600      132803 :     pose.header = msg->header;
+     601      132803 :     pose.pose   = msg->pose.pose;
+     602             : 
+     603      132803 :     auto res = ch_->transformer->transformSingle(pose, frame_id);
+     604      132803 :     if (res) {
+     605      124552 :       msg_out.pose.pose = res->pose;
+     606      124552 :       ph.publish(msg_out);
+     607             :     } else {
+     608        8251 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Could not transform pose to %s. Not republishing odom in this frame.", getPrintName().c_str(), frame_id.c_str());
+     609        8251 :       return;
+     610             :     }
+     611             :   }
+     612             : 
+     613             :   /*//}*/
+     614             : };
+     615             : 
+     616             : /*//}*/
+     617             : 
+     618             : }  // namespace mrs_uav_managers
+     619             : 
+     620             : #endif
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.overview.html b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.overview.html new file mode 100644 index 0000000000..bceb06a24c --- /dev/null +++ b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.overview.html @@ -0,0 +1,175 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/include/transform_manager/tf_source.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.png b/mrs_uav_managers/include/transform_manager/tf_source.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..57eba571215fb10005746608a369fcc6556ed150 GIT binary patch literal 2210 zcmV;T2wnGyP)4->0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpg=oWMbxP^8H&i=P@m1hzX2xzCBbIUi=W9R&aya*7Y!#vN^JP-Be zfam93Ky^Ebjh5v76z4Ll6e#j=1<3bKfofqybPg10D%*vwD~j*y`jX2LidRSxidDAM zz~-*GrUv$@!5Tty`+d@;t|#C3SSB7&clSL{(^d5Wf?9uw5x3Iavbna}Ph)@lWw^3! zSkATvVBP`&M|M0tXetYQ&@<^?lTizQaa}9g0gTK;ru^R!P*3rc+0t9fKqbXsn^9vO zRa;K64UpSlUAopL^#aKykLZ?qiKYPtF^JhU*t!OcPJw;VQ0{!!kE8w2w(sfFw-dNO z6zu|!f)80eYDn5KbCbf&=@STejPG?@J;V0$zqy4JbbIk?6!*^Ks= zkNPV0Y^({r%<(P;5ft;}~+d4D0hQ~N7P#|mUciZ}2dJ7wSa{oY6 z-1Gc-{_H<{{O&*Z9|SB;aq79SNjeBVlszwYf33kbXxsfRB|!6m=- zQm=JB^-{ku)K^!{$Z+ynDoX8vZl%Kr_#sl4&An`@=Yj%b!FAOsUkD6^*1Crn71&>S5XsK-#(ZB5TQB4A9gnUAQx zLzN|pcP@qMiYQe#a<0z5bwc{l|QT3~@9ve{Ii=tQ6=$y(qA z_f4HdniD&IQg7qcJ|RVN;LFnGf!DeYS74ZMVpFMaiWHi_L!~ZM*QM@Qj zL)S>%L5h6jP+<6~1YV)oEDWLAM=YbrG27k)dw&!dP~?FDuK^Y*@{CRXQX#>3XE_QW zJ{7qPZ6UcgvZqrAnp;#_}r)TJb<60k#$wevy? zvTN%~_FA#+X<1(PC|sGIje3-K>^dHiKW2&Mq*?Eew2Fj3Il~zD^BazP7C(UfUI=1o z4U|e|=WJOf><}q%JZW&qBUIg;4@<&u()epZ7@o&W7(tr5g#m{opx}vj+$`MUa)wy_ zpdE#8I~k($j7*%)GwgwnX^gnvd8Rba09cXYJM-Zq9H<37VuvB;$!VN*>G4L(1RBTp z^OrjqHyQBw3_zJ8bN(7{XeplaF|}5$l-J^H!pRxXc9DSw0`u%OR*z5uk)3@Ym*^7ai1xY)cDzbCzCEApng#=R2*edc`2q<5VWYtP zL4bRr_=1Rw=b4gIlpPFMCbujP)YW`g^_(llitwba`FY;N-tvN4uBEALM=@Y&%|}>@ z^ChgNa-JFVh#c*?=S~~-sL_v=e4Z4O91%wJ3MWiWk|2!6E2z|>S2Rie<`v}V=A=8w z<#sWwpS2ZzX&M>|#n?kk=xq4(Tgmt*7f4l$*ESE=>X9sw_{oq0(@aiU8?Z~^k|oJ- z&6j!x0{dc*wVkWM-Khy*Xc@AmkS)XNS^ydfG*FLf%lJ;WS|wo9^ue#R2(Zb7*(#=c zZ~>`vPP-mXJKL2BDN{WKS{{wRq|YIJXRZpx$>*0!Q`gc{TpaM3>u^9R0x%Tw(Zd6? z2;g|=4gkfP57uwvon8$R(AY;eDaBSEKYNNx+Nc2kl|ZU)Ks}Lr@|bb;@gijJ-we&f znuXAeWAi^3n$_+07~YjjdB#Pa+i~vp;(R1)fGQw^VJGTQ3X*w*FZXxl{F{Ap{mg(S z$E9vgzQ1?M1SeG`+Bo6M7TVg-^zMXjrs-OaG`}7j{EgMVOE8}S0sGP zxp)C6thI`YF-~6;c<1ZU$6G#gK-L#t${rpJ@m?#6cf5ACc1nfP=drxg^Hn$t{)-l zRe(4=BI)-`7~#zBXQ-UnMai0kK{TE+@KKEtv1^ul2B?b^k8hAFca1v!@$R*UgtzPZ z=Q)V|mfg$^9Db*%n}r{PuDYkG$}U#T*ET>i+R*qqmTPaAn=wK9YPs1}c6U)sbmf;% zl1?w~|3J6r*KrJ2GH^;jpw>u((S`djpNe(AuLqt_#YoC|BT(IN1%An4RY|U#%g`8H zLjvL)ff_gv#lCt=!fCr!hGx{W@7iAZ_cxpu=i#<=hcjm%i}|BXq0dm?WgS@VH#Zdi ze>wVnLI$Z8o8dDiH^pW`?;Dy6-U9+@x?^xdK@X%43^KyoP?%Btz|gD)s@pe)7uz?6 zmxA~w#PAEJ04`4gUKFXze7y1$VBHE&R?$bGR_R#0`v5|GrYqK7ds%A~jrcB?J|0&- z6TQM7e2%nw*v!3wr4Fo}RA`R5vkbf0ybG>Q8e=clEoRv3DYoGS&-my^p>kZ`_ws|* kp7m{t!CIWK8k?5Le~K&MN-hp_ivR!s07*qoM6N<$f}cz^EdT%j literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/src/constraint_manager.cpp.func-sort-c.html b/mrs_uav_managers/src/constraint_manager.cpp.func-sort-c.html new file mode 100644 index 0000000000..fea25d4425 --- /dev/null +++ b/mrs_uav_managers/src/constraint_manager.cpp.func-sort-c.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:15223265.5 %
Date:2024-04-09 22:03:33Functions:6875.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::constraint_manager::ConstraintManager::callbackSetConstraints(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)0
mrs_uav_managers::constraint_manager::ConstraintManager::callbackConstraintsOverride(mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> >&, mrs_msgs::ConstraintsOverrideResponse_<std::allocator<void> >&)0
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_managers::constraint_manager::ConstraintManager::setConstraints(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)82
mrs_uav_managers::constraint_manager::ConstraintManager::onInit()82
mrs_uav_managers::constraint_manager::ConstraintManager::timerDiagnostics(ros::TimerEvent const&)1922
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&)2206
mrs_uav_managers::constraint_manager::ConstraintManager::timerConstraintManagement(ros::TimerEvent const&)19619
+
+
+ + + +
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..4521ef43ab --- /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:15223265.5 %
Date:2024-04-09 22:03:33Functions:6875.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_managers::constraint_manager::ConstraintManager::setConstraints(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)82
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&)2206
mrs_uav_managers::constraint_manager::ConstraintManager::timerDiagnostics(ros::TimerEvent const&)1922
mrs_uav_managers::constraint_manager::ConstraintManager::callbackSetConstraints(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)0
mrs_uav_managers::constraint_manager::ConstraintManager::timerConstraintManagement(ros::TimerEvent const&)19619
mrs_uav_managers::constraint_manager::ConstraintManager::callbackConstraintsOverride(mrs_msgs::ConstraintsOverrideRequest_<std::allocator<void> >&, mrs_msgs::ConstraintsOverrideResponse_<std::allocator<void> >&)0
mrs_uav_managers::constraint_manager::ConstraintManager::onInit()82
+
+
+ + + +
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..a5c23caf13 --- /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:15223265.5 %
Date:2024-04-09 22:03:33Functions:6875.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          82 : void ConstraintManager::onInit() {
+     120             : 
+     121          82 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     122             : 
+     123          82 :   ros::Time::waitForValid();
+     124             : 
+     125          82 :   ROS_INFO("[ConstraintManager]: initializing");
+     126             : 
+     127             :   // | ------------------------- params ------------------------- |
+     128             : 
+     129         164 :   mrs_lib::ParamLoader param_loader(nh_, "ConstraintManager");
+     130             : 
+     131         164 :   std::string custom_config_path;
+     132         164 :   std::string platform_config_path;
+     133             : 
+     134          82 :   param_loader.loadParam("custom_config", custom_config_path);
+     135          82 :   param_loader.loadParam("platform_config", platform_config_path);
+     136             : 
+     137          82 :   if (custom_config_path != "") {
+     138          82 :     param_loader.addYamlFile(custom_config_path);
+     139             :   }
+     140             : 
+     141          82 :   if (platform_config_path != "") {
+     142          82 :     param_loader.addYamlFile(platform_config_path);
+     143             :   }
+     144             : 
+     145          82 :   param_loader.addYamlFileFromParam("private_config");
+     146          82 :   param_loader.addYamlFileFromParam("public_config");
+     147          82 :   param_loader.addYamlFileFromParam("public_constraints");
+     148             : 
+     149         164 :   const std::string yaml_prefix = "mrs_uav_managers/constraint_manager/";
+     150             : 
+     151             :   // params passed from the launch file are not prefixed
+     152          82 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     153             : 
+     154          82 :   param_loader.loadParam(yaml_prefix + "constraints", _constraint_names_);
+     155             : 
+     156          82 :   param_loader.loadParam(yaml_prefix + "estimator_types", _estimator_type_names_);
+     157             : 
+     158          82 :   param_loader.loadParam(yaml_prefix + "rate", _constraint_management_rate_);
+     159          82 :   param_loader.loadParam(yaml_prefix + "diagnostics_rate", _diagnostics_rate_);
+     160             : 
+     161          82 :   std::vector<std::string>::iterator it;
+     162             : 
+     163             :   // loading constraint names
+     164         328 :   for (it = _constraint_names_.begin(); it != _constraint_names_.end(); ++it) {
+     165             : 
+     166         246 :     ROS_INFO_STREAM("[ConstraintManager]: loading constraints '" << *it << "'");
+     167             : 
+     168         246 :     mrs_msgs::DynamicsConstraintsSrvRequest new_constraints;
+     169             : 
+     170         246 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/speed", new_constraints.constraints.horizontal_speed);
+     171         246 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/acceleration", new_constraints.constraints.horizontal_acceleration);
+     172         246 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/jerk", new_constraints.constraints.horizontal_jerk);
+     173         246 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/snap", new_constraints.constraints.horizontal_snap);
+     174             : 
+     175         246 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/speed", new_constraints.constraints.vertical_ascending_speed);
+     176         246 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/acceleration", new_constraints.constraints.vertical_ascending_acceleration);
+     177         246 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/jerk", new_constraints.constraints.vertical_ascending_jerk);
+     178         246 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ascending/snap", new_constraints.constraints.vertical_ascending_snap);
+     179             : 
+     180         246 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/speed", new_constraints.constraints.vertical_descending_speed);
+     181         246 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/acceleration", new_constraints.constraints.vertical_descending_acceleration);
+     182         246 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/jerk", new_constraints.constraints.vertical_descending_jerk);
+     183         246 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/descending/snap", new_constraints.constraints.vertical_descending_snap);
+     184             : 
+     185         246 :     param_loader.loadParam(yaml_prefix + *it + "/heading/speed", new_constraints.constraints.heading_speed);
+     186         246 :     param_loader.loadParam(yaml_prefix + *it + "/heading/acceleration", new_constraints.constraints.heading_acceleration);
+     187         246 :     param_loader.loadParam(yaml_prefix + *it + "/heading/jerk", new_constraints.constraints.heading_jerk);
+     188         246 :     param_loader.loadParam(yaml_prefix + *it + "/heading/snap", new_constraints.constraints.heading_snap);
+     189             : 
+     190         246 :     param_loader.loadParam(yaml_prefix + *it + "/angular_speed/roll", new_constraints.constraints.roll_rate);
+     191         246 :     param_loader.loadParam(yaml_prefix + *it + "/angular_speed/pitch", new_constraints.constraints.pitch_rate);
+     192         246 :     param_loader.loadParam(yaml_prefix + *it + "/angular_speed/yaw", new_constraints.constraints.yaw_rate);
+     193             : 
+     194             :     double tilt_deg;
+     195             : 
+     196         246 :     param_loader.loadParam(yaml_prefix + *it + "/tilt", tilt_deg);
+     197             : 
+     198         246 :     new_constraints.constraints.tilt = M_PI * (tilt_deg / 180.0);
+     199             : 
+     200         246 :     _constraints_.insert(std::pair<std::string, mrs_msgs::DynamicsConstraintsSrvRequest>(*it, new_constraints));
+     201             :   }
+     202             : 
+     203             :   // loading the allowed constraints lists
+     204         656 :   for (it = _estimator_type_names_.begin(); it != _estimator_type_names_.end(); ++it) {
+     205             : 
+     206         574 :     std::vector<std::string> temp_vector;
+     207         574 :     param_loader.loadParam(yaml_prefix + "allowed_constraints/" + *it, temp_vector);
+     208             : 
+     209         574 :     std::vector<std::string>::iterator it2;
+     210        2120 :     for (it2 = temp_vector.begin(); it2 != temp_vector.end(); ++it2) {
+     211        1546 :       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         574 :     _map_type_allowed_constraints_.insert(std::pair<std::string, std::vector<std::string>>(*it, temp_vector));
+     218             :   }
+     219             : 
+     220             :   // loading the default constraints
+     221         656 :   for (it = _estimator_type_names_.begin(); it != _estimator_type_names_.end(); ++it) {
+     222             : 
+     223         574 :     std::string temp_str;
+     224         574 :     param_loader.loadParam(yaml_prefix + "default_constraints/" + *it, temp_str);
+     225             : 
+     226         574 :     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         574 :     _map_type_default_constraints_.insert(std::pair<std::string, std::string>(*it, temp_str));
+     232             :   }
+     233             : 
+     234          82 :   ROS_INFO("[ConstraintManager]: done loading dynamic params");
+     235             : 
+     236          82 :   current_constraints_ = "";
+     237          82 :   last_estimator_name_ = "";
+     238             : 
+     239             :   // | ------------------------ services ------------------------ |
+     240             : 
+     241          82 :   service_server_set_constraints_ = nh_.advertiseService("set_constraints_in", &ConstraintManager::callbackSetConstraints, this);
+     242             : 
+     243          82 :   service_server_constraints_override_ = nh_.advertiseService("constraints_override_in", &ConstraintManager::callbackConstraintsOverride, this);
+     244             : 
+     245          82 :   sc_set_constraints_ = mrs_lib::ServiceClientHandler<mrs_msgs::DynamicsConstraintsSrv>(nh_, "set_constraints_out");
+     246             : 
+     247             :   // | ----------------------- subscribers ---------------------- |
+     248             : 
+     249         164 :   mrs_lib::SubscribeHandlerOptions shopts;
+     250          82 :   shopts.nh                 = nh_;
+     251          82 :   shopts.node_name          = "ConstraintManager";
+     252          82 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     253          82 :   shopts.threadsafe         = true;
+     254          82 :   shopts.autostart          = true;
+     255          82 :   shopts.queue_size         = 10;
+     256          82 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     257             : 
+     258          82 :   sh_estimation_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "estimation_diagnostics_in");
+     259             : 
+     260             :   // | ----------------------- publishers ----------------------- |
+     261             : 
+     262          82 :   ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::ConstraintManagerDiagnostics>(nh_, "diagnostics_out", 10);
+     263             : 
+     264             :   // | ------------------------- timers ------------------------- |
+     265             : 
+     266          82 :   timer_constraint_management_ = nh_.createTimer(ros::Rate(_constraint_management_rate_), &ConstraintManager::timerConstraintManagement, this);
+     267          82 :   timer_diagnostics_           = nh_.createTimer(ros::Rate(_diagnostics_rate_), &ConstraintManager::timerDiagnostics, this);
+     268             : 
+     269             :   // | ------------------------ profiler ------------------------ |
+     270             : 
+     271          82 :   profiler_ = mrs_lib::Profiler(nh_, "ConstraintManager", _profiler_enabled_);
+     272             : 
+     273             :   // | ------------------- scope timer logger ------------------- |
+     274             : 
+     275          82 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     276         246 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     277          82 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     278             : 
+     279             :   // | ----------------------- finish init ---------------------- |
+     280             : 
+     281          82 :   if (!param_loader.loadedSuccessfully()) {
+     282           0 :     ROS_ERROR("[ConstraintManager]: Could not load all parameters!");
+     283           0 :     ros::shutdown();
+     284             :   }
+     285             : 
+     286          82 :   is_initialized_ = true;
+     287             : 
+     288          82 :   ROS_INFO("[ConstraintManager]: initialized");
+     289             : 
+     290          82 :   ROS_DEBUG("[ConstraintManager]: debug output is enabled");
+     291          82 : }
+     292             : 
+     293             : //}
+     294             : 
+     295             : // --------------------------------------------------------------
+     296             : // |                           methods                          |
+     297             : // --------------------------------------------------------------
+     298             : 
+     299             : /* setConstraints() //{ */
+     300             : 
+     301          82 : bool ConstraintManager::setConstraints(std::string constraints_name) {
+     302             : 
+     303          82 :   std::map<std::string, mrs_msgs::DynamicsConstraintsSrvRequest>::iterator it;
+     304          82 :   it = _constraints_.find(constraints_name);
+     305             : 
+     306          82 :   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         164 :   mrs_msgs::DynamicsConstraintsSrv srv_call;
+     312             : 
+     313          82 :   srv_call.request = it->second;
+     314             : 
+     315          82 :   if (override_constraints_) {
+     316             : 
+     317           0 :     auto constraints_override = mrs_lib::get_mutexed(mutex_constraints_override_, constraints_override_);
+     318             : 
+     319           0 :     if (constraints_override.acceleration_horizontal > 0 &&
+     320           0 :         constraints_override.acceleration_horizontal <= srv_call.request.constraints.horizontal_acceleration) {
+     321           0 :       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           0 :     if (constraints_override.acceleration_vertical > 0 &&
+     327           0 :         constraints_override.acceleration_vertical <= srv_call.request.constraints.vertical_ascending_acceleration &&
+     328           0 :         constraints_override.acceleration_vertical <= srv_call.request.constraints.vertical_descending_acceleration) {
+     329           0 :       srv_call.request.constraints.vertical_ascending_acceleration  = constraints_override.acceleration_vertical;
+     330           0 :       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          82 :   bool res = sc_set_constraints_.call(srv_call);
+     337             : 
+     338          82 :   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          82 :     if (srv_call.response.success) {
+     346             : 
+     347          82 :       mrs_lib::set_mutexed(mutex_current_constraints_, constraints_name, current_constraints_);
+     348          82 :       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           0 : bool ConstraintManager::callbackSetConstraints(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+     369             : 
+     370           0 :   if (!is_initialized_) {
+     371           0 :     return false;
+     372             :   }
+     373             : 
+     374           0 :   std::stringstream ss;
+     375             : 
+     376           0 :   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           0 :   auto estimation_diagnostics = sh_estimation_diag_.getMsg();
+     388             : 
+     389           0 :   if (!stringInVector(req.value, _constraint_names_)) {
+     390             : 
+     391           0 :     ss << "the constraints '" << req.value.c_str() << "' do not exist (in the ConstraintManager's config)";
+     392             : 
+     393           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ConstraintManager]: " << ss.str());
+     394             : 
+     395           0 :     res.message = ss.str();
+     396           0 :     res.success = false;
+     397           0 :     return true;
+     398             :   }
+     399             : 
+     400           0 :   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           0 :   override_constraints_ = false;
+     412             : 
+     413             :   // try to set the constraints
+     414           0 :   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           0 :     ss << "the constraints '" << req.value.c_str() << "' were set";
+     427             : 
+     428           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[ConstraintManager]: " << ss.str());
+     429             : 
+     430           0 :     res.message = ss.str();
+     431           0 :     res.success = true;
+     432           0 :     return true;
+     433             :   }
+     434             : }
+     435             : 
+     436             : //}
+     437             : 
+     438             : /* callackConstraintsOverride() //{ */
+     439             : 
+     440           0 : bool ConstraintManager::callbackConstraintsOverride(mrs_msgs::ConstraintsOverride::Request& req, mrs_msgs::ConstraintsOverride::Response& res) {
+     441             : 
+     442           0 :   if (!is_initialized_) {
+     443           0 :     return false;
+     444             :   }
+     445             : 
+     446             :   {
+     447           0 :     std::scoped_lock lock(mutex_constraints_override_);
+     448             : 
+     449           0 :     constraints_override_ = req;
+     450             :   }
+     451             : 
+     452           0 :   override_constraints_         = true;
+     453           0 :   constraints_override_updated_ = true;
+     454             : 
+     455           0 :   ROS_INFO_THROTTLE(0.1, "[ConstraintManager]: setting constraints override");
+     456             : 
+     457           0 :   res.message = "override set";
+     458           0 :   res.success = true;
+     459             : 
+     460           0 :   return true;
+     461             : }
+     462             : 
+     463             : //}
+     464             : 
+     465             : // --------------------------------------------------------------
+     466             : // |                           timers                           |
+     467             : // --------------------------------------------------------------
+     468             : 
+     469             : /* timerConstraintManagement() //{ */
+     470             : 
+     471       19619 : void ConstraintManager::timerConstraintManagement(const ros::TimerEvent& event) {
+     472             : 
+     473       19619 :   if (!is_initialized_) {
+     474        4254 :     return;
+     475             :   }
+     476             : 
+     477       39238 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerConstraintManagement", _constraint_management_rate_, 0.01, event);
+     478       39238 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ContraintManager::timerConstraintManagement", scope_timer_logger_, scope_timer_enabled_);
+     479             : 
+     480       19619 :   auto current_constraints = mrs_lib::get_mutexed(mutex_current_constraints_, current_constraints_);
+     481       19619 :   auto last_estimator_name = mrs_lib::get_mutexed(mutex_last_estimator_name_, last_estimator_name_);
+     482             : 
+     483       19619 :   if (!sh_estimation_diag_.hasMsg()) {
+     484        4254 :     return;
+     485             :   }
+     486             : 
+     487       15365 :   auto estimation_diagnostics = sh_estimation_diag_.getMsg();
+     488             : 
+     489             :   // | --- automatically set constraints when the state estimator changes -- |
+     490       15365 :   if (estimation_diagnostics->current_state_estimator != last_estimator_name_) {
+     491             : 
+     492          86 :     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          86 :     std::map<std::string, std::string>::iterator it;
+     496          86 :     it = _map_type_default_constraints_.find(estimation_diagnostics->current_state_estimator);
+     497             : 
+     498          86 :     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          86 :       if (stringInVector(current_constraints, _map_type_allowed_constraints_.at(estimation_diagnostics->current_state_estimator))) {
+     507             : 
+     508           4 :         last_estimator_name = estimation_diagnostics->current_state_estimator;
+     509             : 
+     510             :         // else, try to set the initial constraints
+     511             :       } else {
+     512             : 
+     513          82 :         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          82 :         if (setConstraints(it->second)) {
+     517             : 
+     518          82 :           last_estimator_name = estimation_diagnostics->current_state_estimator;
+     519             : 
+     520          82 :           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       15365 :   if (constraints_override_updated_) {
+     531             : 
+     532           0 :     std::map<std::string, std::string>::iterator it;
+     533           0 :     it = _map_type_default_constraints_.find(last_estimator_name_);
+     534             : 
+     535           0 :     ROS_INFO_THROTTLE(0.1, "[ConstraintManager]: re-setting constraints with user value override");
+     536             : 
+     537           0 :     if (setConstraints(it->second)) {
+     538           0 :       constraints_override_updated_ = false;
+     539             :     } else {
+     540           0 :       ROS_WARN_THROTTLE(1.0, "[ConstraintManager]: could not re-set the constraints!");
+     541             :     }
+     542             :   }
+     543             : 
+     544       15365 :   mrs_lib::set_mutexed(mutex_last_estimator_name_, last_estimator_name, last_estimator_name_);
+     545             : }
+     546             : 
+     547             : //}
+     548             : 
+     549             : /* timerDiagnostics() //{ */
+     550             : 
+     551        1922 : void ConstraintManager::timerDiagnostics(const ros::TimerEvent& event) {
+     552             : 
+     553        1922 :   if (!is_initialized_) {
+     554         412 :     return;
+     555             :   }
+     556             : 
+     557        3844 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerDiagnostics", _diagnostics_rate_, 0.01, event);
+     558        3844 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ContraintManager::timerDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+     559             : 
+     560        1922 :   if (!sh_estimation_diag_.hasMsg()) {
+     561         411 :     ROS_WARN_THROTTLE(10.0, "[ConstraintManager]: can not do constraint management, missing estimation diagnostics!");
+     562         411 :     return;
+     563             :   }
+     564             : 
+     565        1511 :   auto estimation_diagnostics = sh_estimation_diag_.getMsg();
+     566             : 
+     567        1511 :   auto current_constraints = mrs_lib::get_mutexed(mutex_current_constraints_, current_constraints_);
+     568             : 
+     569        1511 :   if (current_constraints == "") {  // this could happend just before timerConstraintManagement() finishes
+     570           1 :     return;
+     571             :   }
+     572             : 
+     573        1510 :   mrs_msgs::ConstraintManagerDiagnostics diagnostics;
+     574             : 
+     575        1510 :   diagnostics.stamp        = ros::Time::now();
+     576        1510 :   diagnostics.current_name = current_constraints;
+     577        1510 :   diagnostics.loaded       = _constraint_names_;
+     578             : 
+     579             :   // get the available constraints
+     580             :   {
+     581        1510 :     std::map<std::string, std::vector<std::string>>::iterator it;
+     582        1510 :     it = _map_type_allowed_constraints_.find(estimation_diagnostics->current_state_estimator);
+     583             : 
+     584        1510 :     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        1510 :       diagnostics.available = it->second;
+     589             :     }
+     590             :   }
+     591             : 
+     592             :   // get the current constraint values
+     593             :   {
+     594        1510 :     std::map<std::string, mrs_msgs::DynamicsConstraintsSrvRequest>::iterator it;
+     595        1510 :     it = _constraints_.find(current_constraints);
+     596             : 
+     597        1510 :     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        1510 :     diagnostics.current_values = it->second.constraints;
+     603             :   }
+     604             : 
+     605        1510 :   ph_diagnostics_.publish(diagnostics);
+     606             : }
+     607             : 
+     608             : //}
+     609             : 
+     610             : // --------------------------------------------------------------
+     611             : // |                          routines                          |
+     612             : // --------------------------------------------------------------
+     613             : 
+     614             : /* stringInVector() //{ */
+     615             : 
+     616        2206 : bool ConstraintManager::stringInVector(const std::string& value, const std::vector<std::string>& vector) {
+     617             : 
+     618        2206 :   if (std::find(vector.begin(), vector.end(), value) == vector.end()) {
+     619          82 :     return false;
+     620             :   } else {
+     621        2124 :     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          82 : 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..fb99dc48e5d6bae7020eb57f2095848a48244a28 GIT binary patch literal 2202 zcmV;L2xa$)P)6Ks=d}Xs~sM&KoqJ=9uL(6oQN=PFH$;rjo zBIxFuM1*oYGh_R-F4!|6NLU*HKBXFt#|*KR$M2~p4z4MF#&->ai}0R=&^@qjrd4gO zlM&;b!cKRD7w?b|^??ZkK+g9_P-z-%84%;>5pNn^RAh()(n#QLOmGs76nTV*Y8eSc zw0vOqZqws*$6~%fd%vn}P?*vGH?;{vyo3>e7m;)sQQ9jJW==zoN?`y)0d-0PtD)aM zPw2USUO2j4scwktYF60Hi*5t+o*-P8Wu@9uj|f|dunsZmpdOKMTP zPod;vOc=XdqCZx3?fNJBJ_#4pn_K&$fs7|aaWl{g&SHo6(LT8C$Itos+4lGn5xxuU zB-~2`fw2EMDVqWyqyCWNay&lexOT(~2$4pb9TzQU8BzruU+ zO&1^GVyjlA0EQ0!)r8Wn`)?tq>S1=#Ow*=I9_p=i=^n$K8nORLZXPMD+;eo1YK9G67(Vw5i7<8AjE5M6``pAe17W3g4nx_7BqrnY!&9zR?)lw(W>8a|=(1 z*dVfD1PTeyHATcDMBJ0dEj&GtHnZx%+9{|9Yv-dPBo5Oz~|L=-B#`-1d3 zhdePwHV#*|OrUnSM*G6Y4SZgD{0JvfVLKFFU2V)MJPjb32~D%X#%@ZXQy)iYN;N>r zuKVlfEi|?xyf^j(gX@8-JAX`|nT3px2Aa6E5Le??6)TQ7?`{uYKis;GXZB4IDn!J+ z4zF0Thx( z&9S?Br+_heBDECJI)w!~o&w_ey!#An*G2aKoTB6uoC$I3WWpE#XlC5RgfJ3!_`4(2 zA^t5UgzDf~P(;n=Ivnae3z0$UJ=}s=={<40XY0fNbpnv`*gx?pkLx{X;HL@6ARE&0&l3vUz-~T-iR=1Q zBf1%-;6#IPMp1Sxn0nrW(1HpR{y7^O+3nDuI61RzZ2t}x_T&tldl}`uk5~(OUkNBU zQ+=}3?UkM;;RXu^Jg@B~THsm)r~GAA5X!qwQRuDVFIy(521U|B9;IEMyMW=FK70Y& z6mA&Xjlq#Fyzokf<@=Q-)#wI*alf0eaqb(#g5^V@N9F)Kt**R|3)m?DG-W+*T1tHJ zk=`$qBEE}z_M3g$lh}-j->xRt;{$oh>5)_1j}G@1UDftz9du61=Luu#E%TBT1FFtv z?4ktHaW*CItVkqGKD^uulc=ZU;kvGqD^$!a4TWVSrm00t^y9R&s1W+0_xZkDPeHe&epgI+9xAAAZEysdw;Iws+1koYzPB>kN=kR=2Ad3cukHc^Z>~(C&!sJjP&RF0;ul c1j#h>AL*NPx#07*qoM6N<$f;FKKT>t<8 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..70a4323566 --- /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-04-09 22:03:33Functions: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&)82
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&)471
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&)477
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&)997
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&)1042
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&)1181
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&)1275
mrs_uav_managers::control_manager::RCChannelToRange(double, double, double)2196
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&)2253
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&)3816
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&)3845
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&)10141
mrs_uav_managers::control_manager::initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> >&, double const&)13719
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&)13719
mrs_uav_managers::control_manager::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)14137
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&)65096
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&)76884
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&)86604
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&)94499
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&)121594
mrs_uav_managers::control_manager::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)131458
+
+
+ + + +
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..4d94a4eed1 --- /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-04-09 22:03:33Functions: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&)1275
mrs_uav_managers::control_manager::getLowestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)14137
mrs_uav_managers::control_manager::extractThrottle(mrs_uav_managers::Controller::ControlOutput const&)131458
mrs_uav_managers::control_manager::getHighestOuput(mrs_uav_managers::control_manager::ControlOutputModalities_t const&)0
mrs_uav_managers::control_manager::RCChannelToRange(double, double, double)2196
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&)121594
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&)10141
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&)13719
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&)86604
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&)76884
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&)13719
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&)3816
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&)94499
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&)477
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&)471
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&)82
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&)1181
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&)65096
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&)3845
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&)1042
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&)2253
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&)997
+
+
+ + + +
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..1dccabf3e7 --- /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-04-09 22:03:33Functions: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        1275 : std::optional<unsigned int> idxInVector(const std::string& str, const std::vector<std::string>& vec) {
+      12             : 
+      13        3573 :   for (unsigned int i = 0; i < vec.size(); i++) {
+      14        3571 :     if (str == vec[i]) {
+      15        1273 :       return {i};
+      16             :     }
+      17             :   }
+      18             : 
+      19           2 :   return {};
+      20             : }
+      21             : 
+      22             : //}
+      23             : 
+      24             : /* validateTrackerCommand() //{ */
+      25             : 
+      26       76884 : bool validateTrackerCommand(const std::optional<mrs_msgs::TrackerCommand>& msg, const std::string& node_name, const std::string& var_name) {
+      27             : 
+      28       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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       76884 :   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         471 : bool validateVelocityReference(const mrs_msgs::VelocityReference& msg, const std::string& node_name, const std::string& var_name) {
+     237             : 
+     238             :   // check velocity
+     239             : 
+     240         471 :   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         471 :   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         471 :   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         471 :   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         471 :   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         471 :   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         471 :   return true;
+     271             : }
+     272             : 
+     273             : //}
+     274             : 
+     275             : /* validateReference() //{ */
+     276             : 
+     277       10141 : bool validateReference(const mrs_msgs::Reference& msg, const std::string& node_name, const std::string& var_name) {
+     278             : 
+     279             :   // check position
+     280             : 
+     281       10141 :   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       10141 :   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       10141 :   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       10141 :   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       10141 :   return true;
+     304             : }
+     305             : 
+     306             : //}
+     307             : 
+     308             : /* validateUavState() //{ */
+     309             : 
+     310      121594 : bool validateUavState(const mrs_msgs::UavState& msg, const std::string& node_name, const std::string& var_name) {
+     311             : 
+     312             :   // check position
+     313             : 
+     314      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   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      121594 :   return true;
+     454             : }
+     455             : 
+     456             : //}
+     457             : 
+     458             : /* RCChannelToRange() //{ */
+     459             : 
+     460        2196 : double RCChannelToRange(double rc_value, double range, double deadband) {
+     461             : 
+     462        2196 :   double tmp_neg1_to_1 = (rc_value - 0.5) * 2.0;
+     463             : 
+     464        2196 :   if (tmp_neg1_to_1 > 1.0) {
+     465           0 :     tmp_neg1_to_1 = 1.0;
+     466        2196 :   } else if (tmp_neg1_to_1 < -1.0) {
+     467           0 :     tmp_neg1_to_1 = -1.0;
+     468             :   }
+     469             : 
+     470             :   // check the deadband
+     471        2196 :   if (tmp_neg1_to_1 < deadband && tmp_neg1_to_1 > -deadband) {
+     472        1725 :     return 0.0;
+     473             :   }
+     474             : 
+     475         471 :   if (tmp_neg1_to_1 > 0) {
+     476             : 
+     477         261 :     double tmp = (tmp_neg1_to_1 - deadband) / (1.0 - deadband);
+     478             : 
+     479         261 :     return range * tmp;
+     480             : 
+     481             :   } else {
+     482             : 
+     483         210 :     double tmp = (-tmp_neg1_to_1 - deadband) / (1.0 - deadband);
+     484             : 
+     485         210 :     return -range * tmp;
+     486             :   }
+     487             : 
+     488             :   return 0.0;
+     489             : }
+     490             : 
+     491             : //}
+     492             : 
+     493             : /* loadDetailedUavModelParams() //{ */
+     494             : 
+     495          82 : 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         164 :   mrs_lib::ParamLoader param_loader(nh, node_name);
+     499             : 
+     500          82 :   if (custom_config != "") {
+     501          82 :     param_loader.addYamlFile(custom_config);
+     502             :   }
+     503             : 
+     504          82 :   if (platform_config != "") {
+     505          82 :     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          82 :   bool detailed_loaded = true;
+     518             : 
+     519          82 :   detailed_loaded *= param_loader.loadParam("uav_mass", mass, 0.0);
+     520             : 
+     521          82 :   detailed_loaded *= param_loader.loadParam("model_params/arm_length", arm_length, 0.0);
+     522          82 :   detailed_loaded *= param_loader.loadParam("model_params/body_height", body_height, 0.0);
+     523             : 
+     524          82 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/force_constant", force_constant, 0.0);
+     525          82 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/torque_constant", torque_constant, 0.0);
+     526          82 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/prop_radius", prop_radius, 0.0);
+     527          82 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/rpm/min", rpm_min, 0.0);
+     528          82 :   detailed_loaded *= param_loader.loadParam("model_params/propulsion/rpm/max", rpm_max, 0.0);
+     529             : 
+     530         164 :   Eigen::MatrixXd allocation_matrix;
+     531             : 
+     532          82 :   detailed_loaded *= param_loader.loadMatrixDynamic("model_params/propulsion/allocation_matrix", allocation_matrix, Eigen::Matrix4d::Identity(), 4, -1);
+     533             : 
+     534          82 :   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          82 :     ROS_INFO("[%s]: detailed UAV model params successfully loaded.", node_name.c_str());
+     542             :   }
+     543             : 
+     544          82 :   int n_motors = allocation_matrix.cols();
+     545             : 
+     546         164 :   DetailedModelParams_t model_params;
+     547             : 
+     548          82 :   model_params.arm_length  = arm_length;
+     549          82 :   model_params.body_height = body_height;
+     550          82 :   model_params.prop_radius = prop_radius;
+     551             : 
+     552          82 :   Eigen::Matrix3d inertia_matrix;
+     553             : 
+     554          82 :   bool inertia_loaded = param_loader.loadMatrixStatic("model_params/inertia_matrix", inertia_matrix, Eigen::Matrix3d::Identity());
+     555             : 
+     556          82 :   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          82 :     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          82 :     model_params.inertia       = Eigen::Matrix3d::Zero();
+     568          82 :     model_params.inertia(0, 0) = mass * (3.0 * arm_length * arm_length + body_height * body_height) / 12.0;
+     569          82 :     model_params.inertia(1, 1) = mass * (3.0 * arm_length * arm_length + body_height * body_height) / 12.0;
+     570          82 :     model_params.inertia(2, 2) = (mass * arm_length * arm_length) / 2.0;
+     571             : 
+     572          82 :     ROS_INFO("[%s]: inertia computed form parameters:", node_name.c_str());
+     573          82 :     ROS_INFO_STREAM(model_params.inertia);
+     574             :   }
+     575             : 
+     576             :   // create the force-torque allocation matrix
+     577          82 :   model_params.force_torque_mixer = allocation_matrix;
+     578          82 :   model_params.force_torque_mixer.row(0) *= arm_length * force_constant;
+     579          82 :   model_params.force_torque_mixer.row(1) *= arm_length * force_constant;
+     580          82 :   model_params.force_torque_mixer.row(2) *= torque_constant * (3.0 * prop_radius) * force_constant;
+     581          82 :   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         164 :       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         416 :   for (int i = 0; i < n_motors; i++) {
+     594         334 :     alloc_tmp.block(i, 0, 1, 2).normalize();
+     595             :   }
+     596             : 
+     597             :   // the 3rd column (yaw)
+     598         416 :   for (int i = 0; i < n_motors; i++) {
+     599         334 :     if (alloc_tmp(i, 2) > 1e-2) {
+     600         167 :       alloc_tmp(i, 2) = 1.0;
+     601         167 :     } else if (alloc_tmp(i, 2) < -1e-2) {
+     602         167 :       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         416 :   for (int i = 0; i < n_motors; i++) {
+     610         334 :     alloc_tmp(i, 3) = 1.0;
+     611             :   }
+     612             : 
+     613          82 :   model_params.control_group_mixer = alloc_tmp;
+     614             : 
+     615          82 :   return model_params;
+     616             : }
+     617             : 
+     618             : //}
+     619             : 
+     620             : /* getLowestOutput() //{ */
+     621             : 
+     622       14137 : CONTROL_OUTPUT getLowestOuput(const ControlOutputModalities_t& outputs) {
+     623             : 
+     624       14137 :   if (outputs.actuators) {
+     625          16 :     return ACTUATORS_CMD;
+     626             :   }
+     627             : 
+     628       14121 :   if (outputs.control_group) {
+     629          16 :     return CONTROL_GROUP;
+     630             :   }
+     631             : 
+     632       14105 :   if (outputs.attitude_rate) {
+     633       13999 :     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      131458 : std::optional<double> extractThrottle(const Controller::ControlOutput& control_output) {
+     707             : 
+     708      131458 :   if (!control_output.control_output) {
+     709       20103 :     return {};
+     710             :   }
+     711             : 
+     712      222710 :   return std::visit(HwApiCmdExtractThrottleVisitor(), control_output.control_output.value());
+     713             : }
+     714             : 
+     715             : //}
+     716             : 
+     717             : // | -------------- validation of HW api commands ------------- |
+     718             : 
+     719             : /* validateControlOutput() //{ */
+     720             : 
+     721       86604 : 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       86604 :   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       86604 :   std::variant<ControlOutputModalities_t> output_modalities_var{output_modalities};
+     730      173208 :   std::variant<std::string>               node_name_var{node_name};
+     731       86604 :   std::variant<std::string>               var_name_var{var_name};
+     732             : 
+     733       86604 :   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        3816 : bool validateHwApiActuatorCmd(const mrs_msgs::HwApiActuatorCmd& msg, const std::string& node_name, const std::string& var_name) {
+     741             : 
+     742       19080 :   for (size_t i = 0; i < msg.motors.size(); i++) {
+     743       15264 :     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        3816 :   return true;
+     750             : }
+     751             : 
+     752             : //}
+     753             : 
+     754             : /* validateHwApiControlGroupCmd() //{ */
+     755             : 
+     756        3845 : bool validateHwApiControlGroupCmd(const mrs_msgs::HwApiControlGroupCmd& msg, const std::string& node_name, const std::string& var_name) {
+     757             : 
+     758        3845 :   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        3845 :   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        3845 :   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        3845 :   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        3845 :   return true;
+     779             : }
+     780             : 
+     781             : //}
+     782             : 
+     783             : /* validateHwApiAttitudeCmd() //{ */
+     784             : 
+     785       94499 : bool validateHwApiAttitudeCmd(const mrs_msgs::HwApiAttitudeCmd& msg, const std::string& node_name, const std::string& var_name) {
+     786             : 
+     787             :   // check the orientation
+     788             : 
+     789       94499 :   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       94499 :   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       94499 :   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       94499 :   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       94499 :   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       94499 :   return true;
+     817             : }
+     818             : 
+     819             : //}
+     820             : 
+     821             : /* validateHwApiAttitudeRateCmd() //{ */
+     822             : 
+     823       65096 : 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       65096 :   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       65096 :   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       65096 :   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       65096 :   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       65096 :   return true;
+     850             : }
+     851             : 
+     852             : //}
+     853             : 
+     854             : /* validateHwApiAccelerationHdgRateCmd() //{ */
+     855             : 
+     856         997 : bool validateHwApiAccelerationHdgRateCmd(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     857             : 
+     858             :   // | ----------------- check the acceleration ----------------- |
+     859             : 
+     860         997 :   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         997 :   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         997 :   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         997 :   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         997 :   return true;
+     883             : }
+     884             : 
+     885             : //}
+     886             : 
+     887             : /* validateHwApiAccelerationHdgCmd() //{ */
+     888             : 
+     889        1042 : bool validateHwApiAccelerationHdgCmd(const mrs_msgs::HwApiAccelerationHdgCmd& msg, const std::string& node_name, const std::string& var_name) {
+     890             : 
+     891             :   // | ----------------- check the acceleration ----------------- |
+     892             : 
+     893        1042 :   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        1042 :   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        1042 :   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        1042 :   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        1042 :   return true;
+     916             : }
+     917             : 
+     918             : //}
+     919             : 
+     920             : /* validateHwApiVelocityHdgRateCmd() //{ */
+     921             : 
+     922        2253 : bool validateHwApiVelocityHdgRateCmd(const mrs_msgs::HwApiVelocityHdgRateCmd& msg, const std::string& node_name, const std::string& var_name) {
+     923             : 
+     924             :   // | ----------------- check the velocity ----------------- |
+     925             : 
+     926        2253 :   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        2253 :   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        2253 :   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        2253 :   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        2253 :   return true;
+     949             : }
+     950             : 
+     951             : //}
+     952             : 
+     953             : /* validateHwApiVelocityHdgCmd() //{ */
+     954             : 
+     955        1181 : bool validateHwApiVelocityHdgCmd(const mrs_msgs::HwApiVelocityHdgCmd& msg, const std::string& node_name, const std::string& var_name) {
+     956             : 
+     957             :   // | ----------------- check the velocity ----------------- |
+     958             : 
+     959        1181 :   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        1181 :   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        1181 :   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        1181 :   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        1181 :   return true;
+     982             : }
+     983             : 
+     984             : //}
+     985             : 
+     986             : /* validateHwApiPositionHdgCmd() //{ */
+     987             : 
+     988         477 : bool validateHwApiPositionCmd(const mrs_msgs::HwApiPositionCmd& msg, const std::string& node_name, const std::string& var_name) {
+     989             : 
+     990             :   // | ----------------- check the position ----------------- |
+     991             : 
+     992         477 :   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         477 :   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         477 :   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         477 :   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         477 :   return true;
+    1015             : }
+    1016             : 
+    1017             : //}
+    1018             : 
+    1019             : // | ------------ initialization of HW api commands ----------- |
+    1020             : 
+    1021             : /* initializeDefaultOutput() //{ */
+    1022             : 
+    1023       13719 : 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       13719 :   CONTROL_OUTPUT lowest_output = getLowestOuput(possible_outputs);
+    1027             : 
+    1028       13719 :   Controller::HwApiOutputVariant output;
+    1029             : 
+    1030       13719 :   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       13719 :     case ATTITUDE_RATE: {
+    1040       13719 :       output = mrs_msgs::HwApiAttitudeRateCmd();
+    1041       13719 :       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       27438 :   std::variant<mrs_msgs::UavState> uav_state_var{uav_state};
+    1070       13719 :   std::variant<double>             min_throttle_var{min_throttle};
+    1071       13719 :   std::variant<double>             n_motors_var{n_motors};
+    1072             : 
+    1073       13719 :   std::visit(HwApiInitializeVisitor(), output, uav_state_var, min_throttle_var, n_motors_var);
+    1074             : 
+    1075       27438 :   return output;
+    1076             : }  // namespace mrs_uav_managers
+    1077             : 
+    1078             : //}
+    1079             : 
+    1080             : /* initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd& msg) //{ */
+    1081             : 
+    1082           0 : void initializeHwApiCmd(mrs_msgs::HwApiActuatorCmd& msg, const double& min_throttle, const double& n_motors) {
+    1083             : 
+    1084           0 :   msg.stamp = ros::Time::now();
+    1085             : 
+    1086           0 :   for (int i = 0; i < n_motors; i++) {
+    1087           0 :     msg.motors.push_back(min_throttle);
+    1088             :   }
+    1089           0 : }
+    1090             : 
+    1091             : //}
+    1092             : 
+    1093             : /* initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd& msg) //{ */
+    1094             : 
+    1095           0 : void initializeHwApiCmd(mrs_msgs::HwApiControlGroupCmd& msg, const double& min_throttle) {
+    1096             : 
+    1097           0 :   msg.stamp = ros::Time::now();
+    1098             : 
+    1099           0 :   msg.roll     = 0;
+    1100           0 :   msg.pitch    = 0;
+    1101           0 :   msg.yaw      = 0;
+    1102           0 :   msg.throttle = min_throttle;
+    1103           0 : }
+    1104             : 
+    1105             : //}
+    1106             : 
+    1107             : /* initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd& msg) //{ */
+    1108             : 
+    1109       13719 : void initializeHwApiCmd(mrs_msgs::HwApiAttitudeRateCmd& msg, const double& min_throttle) {
+    1110             : 
+    1111       13719 :   msg.stamp = ros::Time::now();
+    1112             : 
+    1113       13719 :   msg.body_rate.x = 0;
+    1114       13719 :   msg.body_rate.y = 0;
+    1115       13719 :   msg.body_rate.z = 0;
+    1116       13719 :   msg.throttle    = min_throttle;
+    1117       13719 : }
+    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..b3860433aecbbd418483ef3e00b664aa390d2497 GIT binary patch literal 2428 zcmV-?34`{DP)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}hJM7iVX4yHlIE2&7it}a> zH44uxerpsW;BHny>Y7Nnk~!LGAx`*zT2Jgq{sNL~xd&qxSlBbVGCon*c}`UDt41j5 zMPg{^Sp_29c$W$3@AnguVzs&w#t7f<74N(ANFnn-`R728c}4_S{R)8mkvWV_l+cV# zcy!Kmjn5!82@{i7_hxp+$P#8|jEv1JNl2CPK}{#C1-qxjW1}zF-_FgIPc%&ex=wNY zKlTdjT!3S06Ht*b;e@@L**POa%?ub>XJ(e#YStj6%E&Ps=SU^|kM~@^b}k3WR|u&x uvUA}asf6X7>nRI->Zc?M@b@@I=e~dVah4|DvU>ag0000 + + + + + + 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-04-09 22:03:33Functions: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..9d04e8ef94 --- /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-04-09 22:03:33Functions: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..017610f9ec --- /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-04-09 22:03:33Functions: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..a9605e6df0 --- /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-04-09 22:03:33Functions: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..fba6b9429e --- /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-04-09 22:03:33Functions: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..01efb0b1c3 --- /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-04-09 22:03:33Functions: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..810f121915 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.func-sort-c.html @@ -0,0 +1,524 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/control_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager - control_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2325365163.7 %
Date:2024-04-09 22:03:33Functions:7711169.4 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::control_manager::ControlManager::parachuteSrv()0
mrs_uav_managers::control_manager::ControlManager::callbackHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackEHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::loadConfigFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)0
mrs_uav_managers::control_manager::ControlManager::timerPirouette(ros::TimerEvent const&)0
mrs_uav_managers::control_manager::ControlManager::callbackGetMinZ(mrs_msgs::GetFloat64Request_<std::allocator<void> >&, mrs_msgs::GetFloat64Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoFcu(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSetMinZ(mrs_msgs::Float64StampedSrvRequest_<std::allocator<void> >&, mrs_msgs::Float64StampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::deployParachute[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::callbackJoystick(boost::shared_ptr<sensor_msgs::Joy_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackParachute(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackPirouette(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSetHeading(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackUseJoystick(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackEnableBumper(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoAltitude(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformPoseSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackUseSafetyArea(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackReferenceTopic(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::stopTrajectoryTracking[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::callbackReferenceService(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3SrvRequest_<std::allocator<void> >&, mrs_msgs::TransformVector3SrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::resumeTrajectoryTracking[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrackerResetStatic(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformReferenceSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackValidateReferenceList(mrs_msgs::ValidateReferenceListRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceListResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackStopTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceTopic(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackResumeTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceTopic(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::hover[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::callbackEland(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::gotoTrajectoryStart[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::startTrajectoryTracking[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::callbackGotoTrajectoryStart(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackStartTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::ehover[abi:cxx11]()2
mrs_uav_managers::control_manager::ControlManager::callbackFailsafe(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)4
mrs_uav_managers::control_manager::ControlManager::setTrajectoryReference[abi:cxx11](mrs_msgs::TrajectoryReference_<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::callbackArm(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)6
mrs_uav_managers::control_manager::ControlManager::timeoutUavState(double const&)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::eland[abi:cxx11]()7
mrs_uav_managers::control_manager::ControlManager::elandSrv()7
mrs_uav_managers::control_manager::ControlManager::getNextEscFailsafeState()8
mrs_uav_managers::control_manager::ControlManager::failsafe[abi:cxx11]()8
mrs_uav_managers::control_manager::ControlManager::changeLandingState(mrs_uav_managers::control_manager::LandingStates_t)10
mrs_uav_managers::control_manager::ControlManager::odometryCallbacksSrv(bool)15
mrs_uav_managers::control_manager::ControlManager::isOffboard()16
mrs_uav_managers::control_manager::ControlManager::arming[abi:cxx11](bool)16
mrs_uav_managers::control_manager::ControlManager::callbackGotoRelative(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)22
mrs_uav_managers::control_manager::ControlManager::callbackGoto(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)25
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)31
mrs_uav_managers::control_manager::ControlManager::setReference[abi:cxx11](mrs_msgs::ReferenceStamped_<std::allocator<void> >)47
mrs_uav_managers::control_manager::ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)78
mrs_uav_managers::control_manager::ControlManager::setCallbacks(bool)79
mrs_uav_managers::control_manager::ControlManager::callbackEnableCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)79
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_managers::control_manager::ControlManager::initialize()82
mrs_uav_managers::control_manager::ControlManager::preinitialize()82
mrs_uav_managers::control_manager::ControlManager::setConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)82
mrs_uav_managers::control_manager::ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&, mrs_msgs::DynamicsConstraintsSrvResponse_<std::allocator<void> >&)82
mrs_uav_managers::control_manager::ControlManager::onInit()82
mrs_uav_managers::control_manager::ControlManager::callbackToggleOutput(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)84
mrs_uav_managers::control_manager::ControlManager::ungripSrv()93
mrs_uav_managers::control_manager::ControlManager::toggleOutput(bool const&)102
mrs_uav_managers::control_manager::ControlManager::bumperGetSectorId(double const&, double const&, double const&)120
mrs_uav_managers::control_manager::ControlManager::timerHwApiCapabilities(ros::TimerEvent const&)143
mrs_uav_managers::control_manager::ControlManager::escalatingFailsafe[abi:cxx11]()145
mrs_uav_managers::control_manager::ControlManager::callbackSwitchController(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)158
mrs_uav_managers::control_manager::ControlManager::callbackSwitchTracker(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)159
mrs_uav_managers::control_manager::ControlManager::initializeControlOutput()172
mrs_uav_managers::control_manager::ControlManager::switchTracker(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)186
mrs_uav_managers::control_manager::ControlManager::switchController(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)187
mrs_uav_managers::control_manager::ControlManager::setConstraintsToTrackers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)255
mrs_uav_managers::control_manager::ControlManager::bumperPushFromObstacle()301
mrs_uav_managers::control_manager::ControlManager::setConstraintsToControllers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)319
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)410
mrs_uav_managers::control_manager::ControlManager::getMass()425
mrs_uav_managers::control_manager::ControlManager::timerEland(ros::TimerEvent const&)426
mrs_uav_managers::control_manager::ControlManager::setVelocityReference[abi:cxx11](mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)471
mrs_uav_managers::control_manager::ControlManager::velocityReferenceToReference(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)471
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::VelocityReferenceStampedSrvResponse_<std::allocator<void> >&)471
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)493
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)518
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)1160
mrs_uav_managers::control_manager::ControlManager::timerBumper(ros::TimerEvent const&)2056
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)9514
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)9660
mrs_uav_managers::control_manager::ControlManager::timerFailsafe(ros::TimerEvent const&)9720
mrs_uav_managers::control_manager::ControlManager::getMaxZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)10770
mrs_uav_managers::control_manager::ControlManager::getMinZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)10770
mrs_uav_managers::control_manager::ControlManager::timerStatus(ros::TimerEvent const&)15041
mrs_uav_managers::control_manager::ControlManager::publishDiagnostics()15124
mrs_uav_managers::control_manager::ControlManager::isFlyingNormally()15431
mrs_uav_managers::control_manager::ControlManager::callbackRC(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)19866
mrs_uav_managers::control_manager::ControlManager::timerJoystick(ros::TimerEvent const&)54238
mrs_uav_managers::control_manager::ControlManager::callbackGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)84778
mrs_uav_managers::control_manager::ControlManager::enforceControllersConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)111083
mrs_uav_managers::control_manager::ControlManager::updateTrackers()111592
mrs_uav_managers::control_manager::ControlManager::asyncControl()120881
mrs_uav_managers::control_manager::ControlManager::updateControllers(mrs_msgs::UavState_<std::allocator<void> > const&)121312
mrs_uav_managers::control_manager::ControlManager::publishControlReferenceOdom(std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, mrs_uav_managers::Controller::ControlOutput const&)121312
mrs_uav_managers::control_manager::ControlManager::publish()121312
mrs_uav_managers::control_manager::ControlManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)145682
mrs_uav_managers::control_manager::ControlManager::timerSafety(ros::TimerEvent const&)276905
mrs_uav_managers::control_manager::ControlManager::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)360222
+
+
+ + + +
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..fe3cd8a217 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.func.html @@ -0,0 +1,524 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/control_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager - control_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:2325365163.7 %
Date:2024-04-09 22:03:33Functions:7711169.4 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()82
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)493
mrs_uav_managers::control_manager::ControlManager::callbackRC(boost::shared_ptr<mrs_msgs::HwApiRcChannels_<std::allocator<void> > const>)19866
mrs_uav_managers::control_manager::ControlManager::initialize()82
mrs_uav_managers::control_manager::ControlManager::isOffboard()16
mrs_uav_managers::control_manager::ControlManager::timerEland(ros::TimerEvent const&)426
mrs_uav_managers::control_manager::ControlManager::callbackArm(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)6
mrs_uav_managers::control_manager::ControlManager::timerBumper(ros::TimerEvent const&)2056
mrs_uav_managers::control_manager::ControlManager::timerSafety(ros::TimerEvent const&)276905
mrs_uav_managers::control_manager::ControlManager::timerStatus(ros::TimerEvent const&)15041
mrs_uav_managers::control_manager::ControlManager::asyncControl()120881
mrs_uav_managers::control_manager::ControlManager::callbackGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)84778
mrs_uav_managers::control_manager::ControlManager::callbackGoto(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)25
mrs_uav_managers::control_manager::ControlManager::parachuteSrv()0
mrs_uav_managers::control_manager::ControlManager::setCallbacks(bool)79
mrs_uav_managers::control_manager::ControlManager::setReference[abi:cxx11](mrs_msgs::ReferenceStamped_<std::allocator<void> >)47
mrs_uav_managers::control_manager::ControlManager::toggleOutput(bool const&)102
mrs_uav_managers::control_manager::ControlManager::callbackEland(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::preinitialize()82
mrs_uav_managers::control_manager::ControlManager::switchTracker(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)186
mrs_uav_managers::control_manager::ControlManager::timerFailsafe(ros::TimerEvent const&)9720
mrs_uav_managers::control_manager::ControlManager::timerJoystick(ros::TimerEvent const&)54238
mrs_uav_managers::control_manager::ControlManager::callbackEHover(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::loadConfigFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)0
mrs_uav_managers::control_manager::ControlManager::setConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)82
mrs_uav_managers::control_manager::ControlManager::timerPirouette(ros::TimerEvent const&)0
mrs_uav_managers::control_manager::ControlManager::updateTrackers()111592
mrs_uav_managers::control_manager::ControlManager::callbackGetMinZ(mrs_msgs::GetFloat64Request_<std::allocator<void> >&, mrs_msgs::GetFloat64Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoFcu(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSetMinZ(mrs_msgs::Float64StampedSrvRequest_<std::allocator<void> >&, mrs_msgs::Float64StampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::deployParachute[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::timeoutUavState(double const&)7
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>)145682
mrs_uav_managers::control_manager::ControlManager::isFlyingNormally()15431
mrs_uav_managers::control_manager::ControlManager::switchController(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)187
mrs_uav_managers::control_manager::ControlManager::bumperGetSectorId(double const&, double const&, double const&)120
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&)121312
mrs_uav_managers::control_manager::ControlManager::callbackSetHeading(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::changeLandingState(mrs_uav_managers::control_manager::LandingStates_t)10
mrs_uav_managers::control_manager::ControlManager::escalatingFailsafe[abi:cxx11]()145
mrs_uav_managers::control_manager::ControlManager::publishDiagnostics()15124
mrs_uav_managers::control_manager::ControlManager::callbackHwApiStatus(boost::shared_ptr<mrs_msgs::HwApiStatus_<std::allocator<void> > const>)360222
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]()1
mrs_uav_managers::control_manager::ControlManager::callbackEnableBumper(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoAltitude(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoRelative(mrs_msgs::Vec4Request_<std::allocator<void> >&, mrs_msgs::Vec4Response_<std::allocator<void> >&)22
mrs_uav_managers::control_manager::ControlManager::callbackToggleOutput(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)84
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&)471
mrs_uav_managers::control_manager::ControlManager::callbackSwitchTracker(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)159
mrs_uav_managers::control_manager::ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformPoseSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackUseSafetyArea(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)9660
mrs_uav_managers::control_manager::ControlManager::isPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)1160
mrs_uav_managers::control_manager::ControlManager::bumperPushFromObstacle()301
mrs_uav_managers::control_manager::ControlManager::callbackReferenceTopic(boost::shared_ptr<mrs_msgs::ReferenceStamped_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> >&, mrs_msgs::DynamicsConstraintsSrvResponse_<std::allocator<void> >&)82
mrs_uav_managers::control_manager::ControlManager::setTrajectoryReference[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)4
mrs_uav_managers::control_manager::ControlManager::stopTrajectoryTracking[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::timerHwApiCapabilities(ros::TimerEvent const&)143
mrs_uav_managers::control_manager::ControlManager::callbackEnableCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)79
mrs_uav_managers::control_manager::ControlManager::getNextEscFailsafeState()8
mrs_uav_managers::control_manager::ControlManager::initializeControlOutput()172
mrs_uav_managers::control_manager::ControlManager::startTrajectoryTracking[abi:cxx11]()1
mrs_uav_managers::control_manager::ControlManager::callbackReferenceService(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackSwitchController(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)158
mrs_uav_managers::control_manager::ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3SrvRequest_<std::allocator<void> >&, mrs_msgs::TransformVector3SrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::resumeTrajectoryTracking[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::setConstraintsToTrackers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)255
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::ReferenceStampedSrvResponse_<std::allocator<void> >&)78
mrs_uav_managers::control_manager::ControlManager::callbackFailsafeEscalating(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)7
mrs_uav_managers::control_manager::ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrackerResetStatic(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrvRequest_<std::allocator<void> >&, mrs_msgs::TransformReferenceSrvResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackGotoTrajectoryStart(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
mrs_uav_managers::control_manager::ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReferenceRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceResponse_<std::allocator<void> >&)9514
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea2d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)31
mrs_uav_managers::control_manager::ControlManager::isPathToPointInSafetyArea3d(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&, mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)518
mrs_uav_managers::control_manager::ControlManager::publishControlReferenceOdom(std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, mrs_uav_managers::Controller::ControlOutput const&)121312
mrs_uav_managers::control_manager::ControlManager::setConstraintsToControllers(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)319
mrs_uav_managers::control_manager::ControlManager::velocityReferenceToReference(mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const&)471
mrs_uav_managers::control_manager::ControlManager::callbackValidateReferenceList(mrs_msgs::ValidateReferenceListRequest_<std::allocator<void> >&, mrs_msgs::ValidateReferenceListResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::enforceControllersConstraints(mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const&)111083
mrs_uav_managers::control_manager::ControlManager::callbackStopTrajectoryTracking(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceTopic(boost::shared_ptr<mrs_msgs::VelocityReferenceStamped_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackStartTrajectoryTracking(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> >&)0
mrs_uav_managers::control_manager::ControlManager::callbackTrajectoryReferenceTopic(boost::shared_ptr<mrs_msgs::TrajectoryReference_<std::allocator<void> > const>)0
mrs_uav_managers::control_manager::ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrvRequest_<std::allocator<void> >&, mrs_msgs::VelocityReferenceStampedSrvResponse_<std::allocator<void> >&)471
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]()7
mrs_uav_managers::control_manager::ControlManager::hover[abi:cxx11]()0
mrs_uav_managers::control_manager::ControlManager::arming[abi:cxx11](bool)16
mrs_uav_managers::control_manager::ControlManager::ehover[abi:cxx11]()2
mrs_uav_managers::control_manager::ControlManager::onInit()82
mrs_uav_managers::control_manager::ControlManager::getMass()425
mrs_uav_managers::control_manager::ControlManager::getMaxZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)10770
mrs_uav_managers::control_manager::ControlManager::getMinZ(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)10770
mrs_uav_managers::control_manager::ControlManager::publish()121312
mrs_uav_managers::control_manager::ControlManager::elandSrv()7
mrs_uav_managers::control_manager::ControlManager::failsafe[abi:cxx11]()8
mrs_uav_managers::control_manager::ControlManager::ungripSrv()93
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)410
+
+
+ + + +
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..f0f584c6d8 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.html @@ -0,0 +1,8839 @@ + + + + + + + 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:2325365163.7 %
Date:2024-04-09 22:03:33Functions:7711169.4 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : #include <nodelet/nodelet.h>
+       6             : 
+       7             : #include <mrs_uav_managers/control_manager/common.h>
+       8             : #include <control_manager/output_publisher.h>
+       9             : 
+      10             : #include <mrs_uav_managers/controller.h>
+      11             : #include <mrs_uav_managers/tracker.h>
+      12             : 
+      13             : #include <mrs_msgs/String.h>
+      14             : #include <mrs_msgs/Float64Stamped.h>
+      15             : #include <mrs_msgs/Float64StampedSrv.h>
+      16             : #include <mrs_msgs/ObstacleSectors.h>
+      17             : #include <mrs_msgs/BoolStamped.h>
+      18             : #include <mrs_msgs/ControlManagerDiagnostics.h>
+      19             : #include <mrs_msgs/DynamicsConstraints.h>
+      20             : #include <mrs_msgs/ControlError.h>
+      21             : #include <mrs_msgs/GetFloat64.h>
+      22             : #include <mrs_msgs/ValidateReference.h>
+      23             : #include <mrs_msgs/ValidateReferenceList.h>
+      24             : #include <mrs_msgs/TrackerCommand.h>
+      25             : #include <mrs_msgs/EstimatorInput.h>
+      26             : 
+      27             : #include <geometry_msgs/Point32.h>
+      28             : #include <geometry_msgs/TwistStamped.h>
+      29             : #include <geometry_msgs/PoseArray.h>
+      30             : #include <geometry_msgs/Vector3Stamped.h>
+      31             : 
+      32             : #include <nav_msgs/Odometry.h>
+      33             : 
+      34             : #include <sensor_msgs/Joy.h>
+      35             : #include <sensor_msgs/NavSatFix.h>
+      36             : 
+      37             : #include <mrs_lib/safety_zone/safety_zone.h>
+      38             : #include <mrs_lib/profiler.h>
+      39             : #include <mrs_lib/param_loader.h>
+      40             : #include <mrs_lib/utils.h>
+      41             : #include <mrs_lib/mutex.h>
+      42             : #include <mrs_lib/transformer.h>
+      43             : #include <mrs_lib/geometry/misc.h>
+      44             : #include <mrs_lib/geometry/cyclic.h>
+      45             : #include <mrs_lib/attitude_converter.h>
+      46             : #include <mrs_lib/subscribe_handler.h>
+      47             : #include <mrs_lib/msg_extractor.h>
+      48             : #include <mrs_lib/quadratic_throttle_model.h>
+      49             : #include <mrs_lib/publisher_handler.h>
+      50             : #include <mrs_lib/service_client_handler.h>
+      51             : 
+      52             : #include <mrs_msgs/HwApiCapabilities.h>
+      53             : #include <mrs_msgs/HwApiStatus.h>
+      54             : #include <mrs_msgs/HwApiRcChannels.h>
+      55             : 
+      56             : #include <mrs_msgs/HwApiActuatorCmd.h>
+      57             : #include <mrs_msgs/HwApiControlGroupCmd.h>
+      58             : #include <mrs_msgs/HwApiAttitudeRateCmd.h>
+      59             : #include <mrs_msgs/HwApiAttitudeCmd.h>
+      60             : #include <mrs_msgs/HwApiAccelerationHdgRateCmd.h>
+      61             : #include <mrs_msgs/HwApiAccelerationHdgCmd.h>
+      62             : #include <mrs_msgs/HwApiVelocityHdgRateCmd.h>
+      63             : #include <mrs_msgs/HwApiVelocityHdgCmd.h>
+      64             : #include <mrs_msgs/HwApiPositionCmd.h>
+      65             : 
+      66             : #include <std_msgs/Float64.h>
+      67             : 
+      68             : #include <future>
+      69             : 
+      70             : #include <pluginlib/class_loader.h>
+      71             : 
+      72             : #include <nodelet/loader.h>
+      73             : 
+      74             : #include <eigen3/Eigen/Eigen>
+      75             : 
+      76             : #include <visualization_msgs/Marker.h>
+      77             : #include <visualization_msgs/MarkerArray.h>
+      78             : 
+      79             : #include <mrs_msgs/Reference.h>
+      80             : #include <mrs_msgs/ReferenceStamped.h>
+      81             : #include <mrs_msgs/ReferenceList.h>
+      82             : #include <mrs_msgs/TrajectoryReference.h>
+      83             : 
+      84             : #include <mrs_msgs/ReferenceStampedSrv.h>
+      85             : #include <mrs_msgs/ReferenceStampedSrvRequest.h>
+      86             : #include <mrs_msgs/ReferenceStampedSrvResponse.h>
+      87             : 
+      88             : #include <mrs_msgs/VelocityReferenceStampedSrv.h>
+      89             : #include <mrs_msgs/VelocityReferenceStampedSrvRequest.h>
+      90             : #include <mrs_msgs/VelocityReferenceStampedSrvResponse.h>
+      91             : 
+      92             : #include <mrs_msgs/TransformReferenceSrv.h>
+      93             : #include <mrs_msgs/TransformReferenceSrvRequest.h>
+      94             : #include <mrs_msgs/TransformReferenceSrvResponse.h>
+      95             : 
+      96             : #include <mrs_msgs/TransformPoseSrv.h>
+      97             : #include <mrs_msgs/TransformPoseSrvRequest.h>
+      98             : #include <mrs_msgs/TransformPoseSrvResponse.h>
+      99             : 
+     100             : #include <mrs_msgs/TransformVector3Srv.h>
+     101             : #include <mrs_msgs/TransformVector3SrvRequest.h>
+     102             : #include <mrs_msgs/TransformVector3SrvResponse.h>
+     103             : 
+     104             : #include <mrs_msgs/Float64StampedSrv.h>
+     105             : #include <mrs_msgs/Float64StampedSrvRequest.h>
+     106             : #include <mrs_msgs/Float64StampedSrvResponse.h>
+     107             : 
+     108             : #include <mrs_msgs/Vec4.h>
+     109             : #include <mrs_msgs/Vec4Request.h>
+     110             : #include <mrs_msgs/Vec4Response.h>
+     111             : 
+     112             : #include <mrs_msgs/Vec1.h>
+     113             : #include <mrs_msgs/Vec1Request.h>
+     114             : #include <mrs_msgs/Vec1Response.h>
+     115             : 
+     116             : //}
+     117             : 
+     118             : /* defines //{ */
+     119             : 
+     120             : #define TAU 2 * M_PI
+     121             : #define REF_X 0
+     122             : #define REF_Y 1
+     123             : #define REF_Z 2
+     124             : #define REF_HEADING 3
+     125             : #define ELAND_STR "eland"
+     126             : #define EHOVER_STR "ehover"
+     127             : #define ESCALATING_FAILSAFE_STR "escalating_failsafe"
+     128             : #define FAILSAFE_STR "failsafe"
+     129             : #define INPUT_UAV_STATE 0
+     130             : #define INPUT_ODOMETRY 1
+     131             : #define RC_DEADBAND 0.2
+     132             : 
+     133             : //}
+     134             : 
+     135             : /* using //{ */
+     136             : 
+     137             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+     138             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+     139             : 
+     140             : using radians  = mrs_lib::geometry::radians;
+     141             : using sradians = mrs_lib::geometry::sradians;
+     142             : 
+     143             : //}
+     144             : 
+     145             : namespace mrs_uav_managers
+     146             : {
+     147             : 
+     148             : namespace control_manager
+     149             : {
+     150             : 
+     151             : /* //{ class ControlManager */
+     152             : 
+     153             : // state machine
+     154             : typedef enum
+     155             : {
+     156             : 
+     157             :   IDLE_STATE,
+     158             :   LANDING_STATE,
+     159             : 
+     160             : } LandingStates_t;
+     161             : 
+     162             : const char* state_names[2] = {"IDLING", "LANDING"};
+     163             : 
+     164             : // state machine
+     165             : typedef enum
+     166             : {
+     167             : 
+     168             :   FCU_FRAME,
+     169             :   RELATIVE_FRAME,
+     170             :   ABSOLUTE_FRAME
+     171             : 
+     172             : } ReferenceFrameType_t;
+     173             : 
+     174             : // state machine
+     175             : typedef enum
+     176             : {
+     177             : 
+     178             :   ESC_NONE_STATE     = 0,
+     179             :   ESC_EHOVER_STATE   = 1,
+     180             :   ESC_ELAND_STATE    = 2,
+     181             :   ESC_FAILSAFE_STATE = 3,
+     182             :   ESC_FINISHED_STATE = 4,
+     183             : 
+     184             : } EscalatingFailsafeStates_t;
+     185             : 
+     186             : /* class ControllerParams() //{ */
+     187             : 
+     188             : class ControllerParams {
+     189             : 
+     190             : public:
+     191             :   ControllerParams(std::string address, std::string name_space, double eland_threshold, double failsafe_threshold, double odometry_innovation_threshold,
+     192             :                    bool human_switchable);
+     193             : 
+     194             : public:
+     195             :   double      failsafe_threshold;
+     196             :   double      eland_threshold;
+     197             :   double      odometry_innovation_threshold;
+     198             :   std::string address;
+     199             :   std::string name_space;
+     200             :   bool        human_switchable;
+     201             : };
+     202             : 
+     203         410 : ControllerParams::ControllerParams(std::string address, std::string name_space, double eland_threshold, double failsafe_threshold,
+     204         410 :                                    double odometry_innovation_threshold, bool human_switchable) {
+     205             : 
+     206         410 :   this->eland_threshold               = eland_threshold;
+     207         410 :   this->odometry_innovation_threshold = odometry_innovation_threshold;
+     208         410 :   this->failsafe_threshold            = failsafe_threshold;
+     209         410 :   this->address                       = address;
+     210         410 :   this->name_space                    = name_space;
+     211         410 :   this->human_switchable              = human_switchable;
+     212         410 : }
+     213             : 
+     214             : //}
+     215             : 
+     216             : /* class TrackerParams() //{ */
+     217             : 
+     218             : class TrackerParams {
+     219             : 
+     220             : public:
+     221             :   TrackerParams(std::string address, std::string name_space, bool human_switchable);
+     222             : 
+     223             : public:
+     224             :   std::string address;
+     225             :   std::string name_space;
+     226             :   bool        human_switchable;
+     227             : };
+     228             : 
+     229         493 : TrackerParams::TrackerParams(std::string address, std::string name_space, bool human_switchable) {
+     230             : 
+     231         493 :   this->address          = address;
+     232         493 :   this->name_space       = name_space;
+     233         493 :   this->human_switchable = human_switchable;
+     234         493 : }
+     235             : 
+     236             : //}
+     237             : 
+     238             : class ControlManager : public nodelet::Nodelet {
+     239             : 
+     240             : public:
+     241             :   virtual void onInit();
+     242             : 
+     243             : private:
+     244             :   ros::NodeHandle   nh_;
+     245             :   std::atomic<bool> is_initialized_ = false;
+     246             :   std::string       _uav_name_;
+     247             :   std::string       _body_frame_;
+     248             : 
+     249             :   std::string _custom_config_;
+     250             :   std::string _platform_config_;
+     251             :   std::string _world_config_;
+     252             :   std::string _network_config_;
+     253             : 
+     254             :   // | --------------- dynamic loading of trackers -------------- |
+     255             : 
+     256             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::Tracker>> tracker_loader_;  // pluginlib loader of dynamically loaded trackers
+     257             :   std::vector<std::string>                                           _tracker_names_;  // list of tracker names
+     258             :   std::map<std::string, TrackerParams>                               trackers_;        // map between tracker names and tracker param
+     259             :   std::vector<boost::shared_ptr<mrs_uav_managers::Tracker>>          tracker_list_;    // list of trackers, routines are callable from this
+     260             :   std::mutex                                                         mutex_tracker_list_;
+     261             : 
+     262             :   // | ------------- dynamic loading of controllers ------------- |
+     263             : 
+     264             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::Controller>> controller_loader_;  // pluginlib loader of dynamically loaded controllers
+     265             :   std::vector<std::string>                                              _controller_names_;  // list of controller names
+     266             :   std::map<std::string, ControllerParams>                               controllers_;        // map between controller names and controller params
+     267             :   std::vector<boost::shared_ptr<mrs_uav_managers::Controller>>          controller_list_;    // list of controllers, routines are callable from this
+     268             :   std::mutex                                                            mutex_controller_list_;
+     269             : 
+     270             :   // | ------------------------- HW API ------------------------- |
+     271             : 
+     272             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities> sh_hw_api_capabilities_;
+     273             : 
+     274             :   OutputPublisher control_output_publisher_;
+     275             : 
+     276             :   ControlOutputModalities_t _hw_api_inputs_;
+     277             : 
+     278             :   double desired_uav_state_rate_ = 100.0;
+     279             : 
+     280             :   // this timer will check till we already got the hardware api diagnostics
+     281             :   // then it will trigger the initialization of the controllers and finish
+     282             :   // the initialization of the ControlManager
+     283             :   ros::Timer timer_hw_api_capabilities_;
+     284             :   void       timerHwApiCapabilities(const ros::TimerEvent& event);
+     285             : 
+     286             :   void preinitialize(void);
+     287             :   void initialize(void);
+     288             : 
+     289             :   // | ------------ tracker and controller switching ------------ |
+     290             : 
+     291             :   std::tuple<bool, std::string> switchController(const std::string& controller_name);
+     292             :   std::tuple<bool, std::string> switchTracker(const std::string& tracker_name);
+     293             : 
+     294             :   // the time of last switching of a tracker or a controller
+     295             :   ros::Time  controller_tracker_switch_time_;
+     296             :   std::mutex mutex_controller_tracker_switch_time_;
+     297             : 
+     298             :   // | -------------------- the transformer  -------------------- |
+     299             : 
+     300             :   std::shared_ptr<mrs_lib::Transformer> transformer_;
+     301             : 
+     302             :   // | ------------------- scope timer logger ------------------- |
+     303             : 
+     304             :   bool                                       scope_timer_enabled_ = false;
+     305             :   std::shared_ptr<mrs_lib::ScopeTimerLogger> scope_timer_logger_;
+     306             : 
+     307             :   // | --------------------- general params --------------------- |
+     308             : 
+     309             :   // defines the type of state input: odometry or uav_state mesasge types
+     310             :   int _state_input_;
+     311             : 
+     312             :   // names of important trackers
+     313             :   std::string _null_tracker_name_;     // null tracker is active when UAV is not in the air
+     314             :   std::string _ehover_tracker_name_;   // ehover tracker is used for emergency hovering
+     315             :   std::string _landoff_tracker_name_;  // landoff is used for landing and takeoff
+     316             : 
+     317             :   // names of important controllers
+     318             :   std::string _failsafe_controller_name_;  // controller used for feed-forward failsafe
+     319             :   std::string _eland_controller_name_;     // controller used for emergancy landing
+     320             : 
+     321             :   // joystick control
+     322             :   bool        _joystick_enabled_ = false;
+     323             :   int         _joystick_mode_;
+     324             :   std::string _joystick_tracker_name_;
+     325             :   std::string _joystick_controller_name_;
+     326             :   std::string _joystick_fallback_tracker_name_;
+     327             :   std::string _joystick_fallback_controller_name_;
+     328             : 
+     329             :   // should disarm after emergancy landing?
+     330             :   bool _eland_disarm_enabled_ = false;
+     331             : 
+     332             :   // enabling the emergency handoff -> will disable eland and failsafe
+     333             :   bool _rc_emergency_handoff_ = false;
+     334             : 
+     335             :   // what throttle should be output when null tracker is active?
+     336             :   double _min_throttle_null_tracker_ = 0.0;
+     337             : 
+     338             :   // rates of all the timers
+     339             :   double _status_timer_rate_   = 0;
+     340             :   double _safety_timer_rate_   = 0;
+     341             :   double _elanding_timer_rate_ = 0;
+     342             :   double _failsafe_timer_rate_ = 0;
+     343             :   double _bumper_timer_rate_   = 0;
+     344             : 
+     345             :   bool _snap_trajectory_to_safety_area_ = false;
+     346             : 
+     347             :   // | -------------- uav_state/odometry subscriber ------------- |
+     348             : 
+     349             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odometry_;
+     350             :   mrs_lib::SubscribeHandler<mrs_msgs::UavState> sh_uav_state_;
+     351             : 
+     352             :   mrs_msgs::UavState uav_state_;
+     353             :   mrs_msgs::UavState previous_uav_state_;
+     354             :   bool               got_uav_state_               = false;
+     355             :   double             _uav_state_max_missing_time_ = 0;  // how long should we tolerate missing state estimate?
+     356             :   double             uav_roll_                    = 0;
+     357             :   double             uav_pitch_                   = 0;
+     358             :   double             uav_yaw_                     = 0;
+     359             :   double             uav_heading_                 = 0;
+     360             :   std::mutex         mutex_uav_state_;
+     361             : 
+     362             :   // odometry hiccup detection
+     363             :   double uav_state_avg_dt_        = 1;
+     364             :   double uav_state_hiccup_factor_ = 1;
+     365             :   int    uav_state_count_         = 0;
+     366             : 
+     367             :   mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix> sh_gnss_;
+     368             : 
+     369             :   // | -------------- safety area max z subscriber -------------- |
+     370             : 
+     371             :   mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped> sh_max_z_;
+     372             : 
+     373             :   // | ------------- odometry innovation subscriber ------------- |
+     374             : 
+     375             :   // odometry innovation is published by the odometry node
+     376             :   // it is used to issue eland if the estimator's input is too wonky
+     377             :   mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odometry_innovation_;
+     378             : 
+     379             :   // | --------------------- common handlers -------------------- |
+     380             : 
+     381             :   // contains handlers that are shared with trackers and controllers
+     382             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers_;
+     383             : 
+     384             :   // | --------------- tracker and controller IDs --------------- |
+     385             : 
+     386             :   // keeping track of currently active controllers and trackers
+     387             :   unsigned int active_tracker_idx_    = 0;
+     388             :   unsigned int active_controller_idx_ = 0;
+     389             : 
+     390             :   // indeces of some notable trackers
+     391             :   unsigned int _ehover_tracker_idx_               = 0;
+     392             :   unsigned int _landoff_tracker_idx_              = 0;
+     393             :   unsigned int _joystick_tracker_idx_             = 0;
+     394             :   unsigned int _joystick_controller_idx_          = 0;
+     395             :   unsigned int _failsafe_controller_idx_          = 0;
+     396             :   unsigned int _joystick_fallback_controller_idx_ = 0;
+     397             :   unsigned int _joystick_fallback_tracker_idx_    = 0;
+     398             :   unsigned int _null_tracker_idx_                 = 0;
+     399             :   unsigned int _eland_controller_idx_             = 0;
+     400             : 
+     401             :   // | -------------- enabling the output publisher ------------- |
+     402             : 
+     403             :   void              toggleOutput(const bool& input);
+     404             :   std::atomic<bool> output_enabled_ = false;
+     405             : 
+     406             :   // | ----------------------- publishers ----------------------- |
+     407             : 
+     408             :   mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics>     ph_controller_diagnostics_;
+     409             :   mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand>            ph_tracker_cmd_;
+     410             :   mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput>            ph_mrs_odom_input_;
+     411             :   mrs_lib::PublisherHandler<nav_msgs::Odometry>                  ph_control_reference_odom_;
+     412             :   mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics> ph_diagnostics_;
+     413             :   mrs_lib::PublisherHandler<std_msgs::Empty>                     ph_offboard_on_;
+     414             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_tilt_error_;
+     415             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_mass_estimate_;
+     416             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_mass_nominal_;
+     417             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_throttle_;
+     418             :   mrs_lib::PublisherHandler<std_msgs::Float64>                   ph_thrust_;
+     419             :   mrs_lib::PublisherHandler<mrs_msgs::ControlError>              ph_control_error_;
+     420             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_safety_area_markers_;
+     421             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_safety_area_coordinates_markers_;
+     422             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>     ph_disturbances_markers_;
+     423             :   mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints>       ph_current_constraints_;
+     424             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_heading_;
+     425             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>            ph_speed_;
+     426             : 
+     427             :   // | --------------------- service servers -------------------- |
+     428             : 
+     429             :   ros::ServiceServer service_server_switch_tracker_;
+     430             :   ros::ServiceServer service_server_switch_controller_;
+     431             :   ros::ServiceServer service_server_reset_tracker_;
+     432             :   ros::ServiceServer service_server_hover_;
+     433             :   ros::ServiceServer service_server_ehover_;
+     434             :   ros::ServiceServer service_server_failsafe_;
+     435             :   ros::ServiceServer service_server_failsafe_escalating_;
+     436             :   ros::ServiceServer service_server_toggle_output_;
+     437             :   ros::ServiceServer service_server_arm_;
+     438             :   ros::ServiceServer service_server_enable_callbacks_;
+     439             :   ros::ServiceServer service_server_set_constraints_;
+     440             :   ros::ServiceServer service_server_use_joystick_;
+     441             :   ros::ServiceServer service_server_use_safety_area_;
+     442             :   ros::ServiceServer service_server_emergency_reference_;
+     443             :   ros::ServiceServer service_server_pirouette_;
+     444             :   ros::ServiceServer service_server_eland_;
+     445             :   ros::ServiceServer service_server_parachute_;
+     446             :   ros::ServiceServer service_server_set_min_z_;
+     447             : 
+     448             :   // human callbable services for references
+     449             :   ros::ServiceServer service_server_goto_;
+     450             :   ros::ServiceServer service_server_goto_fcu_;
+     451             :   ros::ServiceServer service_server_goto_relative_;
+     452             :   ros::ServiceServer service_server_goto_altitude_;
+     453             :   ros::ServiceServer service_server_set_heading_;
+     454             :   ros::ServiceServer service_server_set_heading_relative_;
+     455             : 
+     456             :   // the reference service and subscriber
+     457             :   ros::ServiceServer                                    service_server_reference_;
+     458             :   mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped> sh_reference_;
+     459             : 
+     460             :   // the velocity reference service and subscriber
+     461             :   ros::ServiceServer                                            service_server_velocity_reference_;
+     462             :   mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped> sh_velocity_reference_;
+     463             : 
+     464             :   // trajectory tracking
+     465             :   ros::ServiceServer                                       service_server_trajectory_reference_;
+     466             :   mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference> sh_trajectory_reference_;
+     467             :   ros::ServiceServer                                       service_server_start_trajectory_tracking_;
+     468             :   ros::ServiceServer                                       service_server_stop_trajectory_tracking_;
+     469             :   ros::ServiceServer                                       service_server_resume_trajectory_tracking_;
+     470             :   ros::ServiceServer                                       service_server_goto_trajectory_start_;
+     471             : 
+     472             :   // transform service servers
+     473             :   ros::ServiceServer service_server_transform_reference_;
+     474             :   ros::ServiceServer service_server_transform_pose_;
+     475             :   ros::ServiceServer service_server_transform_vector3_;
+     476             : 
+     477             :   // safety area services
+     478             :   ros::ServiceServer service_server_validate_reference_;
+     479             :   ros::ServiceServer service_server_validate_reference_2d_;
+     480             :   ros::ServiceServer service_server_validate_reference_list_;
+     481             : 
+     482             :   // bumper service servers
+     483             :   ros::ServiceServer service_server_bumper_enabler_;
+     484             :   ros::ServiceServer service_server_bumper_repulsion_enabler_;
+     485             : 
+     486             :   // service clients
+     487             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_arming_;
+     488             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_eland_;
+     489             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_shutdown_;
+     490             :   mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_set_odometry_callbacks_;
+     491             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_parachute_;
+     492             : 
+     493             :   // safety area min z servers
+     494             :   ros::ServiceServer service_server_get_min_z_;
+     495             : 
+     496             :   // | --------- trackers' and controllers' last results -------- |
+     497             : 
+     498             :   // the last result of an active tracker
+     499             :   std::optional<mrs_msgs::TrackerCommand> last_tracker_cmd_;
+     500             :   std::mutex                              mutex_last_tracker_cmd_;
+     501             : 
+     502             :   // the last result of an active controller
+     503             :   Controller::ControlOutput last_control_output_;
+     504             :   std::mutex                mutex_last_control_output_;
+     505             : 
+     506             :   // | -------------- HW API diagnostics subscriber ------------- |
+     507             : 
+     508             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus> sh_hw_api_status_;
+     509             : 
+     510             :   bool offboard_mode_          = false;
+     511             :   bool offboard_mode_was_true_ = false;  // if it was ever true
+     512             :   bool armed_                  = false;
+     513             : 
+     514             :   // | -------------------- throttle and mass ------------------- |
+     515             : 
+     516             :   // throttle mass estimation during eland
+     517             :   double    throttle_mass_estimate_   = 0;
+     518             :   bool      throttle_under_threshold_ = false;
+     519             :   ros::Time throttle_mass_estimate_first_time_;
+     520             : 
+     521             :   // | ---------------------- safety params --------------------- |
+     522             : 
+     523             :   // failsafe when tilt error is too large
+     524             :   bool   _tilt_error_disarm_enabled_;
+     525             :   double _tilt_error_disarm_timeout_;
+     526             :   double _tilt_error_disarm_threshold_;
+     527             : 
+     528             :   ros::Time tilt_error_disarm_time_;
+     529             :   bool      tilt_error_disarm_over_thr_ = false;
+     530             : 
+     531             :   // elanding when tilt error is too large
+     532             :   bool   _tilt_limit_eland_enabled_;
+     533             :   double _tilt_limit_eland_ = 0;  // [rad]
+     534             : 
+     535             :   // disarming when tilt error is too large
+     536             :   bool   _tilt_limit_disarm_enabled_;
+     537             :   double _tilt_limit_disarm_ = 0;  // [rad]
+     538             : 
+     539             :   // elanding when yaw error is too large
+     540             :   bool   _yaw_error_eland_enabled_;
+     541             :   double _yaw_error_eland_ = 0;  // [rad]
+     542             : 
+     543             :   // keeping track of control errors
+     544             :   std::optional<double> tilt_error_ = 0;
+     545             :   std::optional<double> yaw_error_  = 0;
+     546             :   std::mutex            mutex_attitude_error_;
+     547             : 
+     548             :   std::optional<Eigen::Vector3d> position_error_;
+     549             :   std::mutex                     mutex_position_error_;
+     550             : 
+     551             :   // control error for triggering failsafe, eland, etc.
+     552             :   // this filled with the current controllers failsafe threshold
+     553             :   double _failsafe_threshold_                = 0;  // control error for triggering failsafe
+     554             :   double _eland_threshold_                   = 0;  // control error for triggering eland
+     555             :   bool   _odometry_innovation_check_enabled_ = false;
+     556             :   double _odometry_innovation_threshold_     = 0;  // innovation size for triggering eland
+     557             : 
+     558             :   bool callbacks_enabled_ = true;
+     559             : 
+     560             :   // | ------------------------ parachute ----------------------- |
+     561             : 
+     562             :   bool _parachute_enabled_ = false;
+     563             : 
+     564             :   std::tuple<bool, std::string> deployParachute(void);
+     565             :   bool                          parachuteSrv(void);
+     566             : 
+     567             :   // | ----------------------- safety area ---------------------- |
+     568             : 
+     569             :   // safety area
+     570             :   std::unique_ptr<mrs_lib::SafetyZone> safety_zone_;
+     571             : 
+     572             :   std::atomic<bool> use_safety_area_ = false;
+     573             : 
+     574             :   std::string _safety_area_horizontal_frame_;
+     575             :   std::string _safety_area_vertical_frame_;
+     576             : 
+     577             :   std::atomic<double> _safety_area_min_z_ = 0;
+     578             : 
+     579             :   double _safety_area_max_z_ = 0;
+     580             : 
+     581             :   // safety area routines
+     582             :   // those are passed to trackers using the common_handlers object
+     583             :   bool   isPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& point);
+     584             :   bool   isPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& point);
+     585             :   bool   isPathToPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& from, const mrs_msgs::ReferenceStamped& to);
+     586             :   bool   isPathToPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& from, const mrs_msgs::ReferenceStamped& to);
+     587             :   double getMinZ(const std::string& frame_id);
+     588             :   double getMaxZ(const std::string& frame_id);
+     589             : 
+     590             :   // | ------------------------ callbacks ----------------------- |
+     591             : 
+     592             :   // topic callbacks
+     593             :   void callbackOdometry(const nav_msgs::Odometry::ConstPtr msg);
+     594             :   void callbackUavState(const mrs_msgs::UavState::ConstPtr msg);
+     595             :   void callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg);
+     596             :   void callbackGNSS(const sensor_msgs::NavSatFix::ConstPtr msg);
+     597             :   void callbackRC(const mrs_msgs::HwApiRcChannels::ConstPtr msg);
+     598             : 
+     599             :   // topic timeouts
+     600             :   void timeoutUavState(const double& missing_for);
+     601             : 
+     602             :   // switching controller and tracker services
+     603             :   bool callbackSwitchTracker(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     604             :   bool callbackSwitchController(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     605             :   bool callbackTrackerResetStatic(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     606             : 
+     607             :   // reference callbacks
+     608             :   void callbackReferenceTopic(const mrs_msgs::ReferenceStamped::ConstPtr msg);
+     609             :   void callbackVelocityReferenceTopic(const mrs_msgs::VelocityReferenceStamped::ConstPtr msg);
+     610             :   void callbackTrajectoryReferenceTopic(const mrs_msgs::TrajectoryReference::ConstPtr msg);
+     611             :   bool callbackGoto(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     612             :   bool callbackGotoFcu(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     613             :   bool callbackGotoRelative(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+     614             :   bool callbackGotoAltitude(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     615             :   bool callbackSetHeading(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     616             :   bool callbackSetHeadingRelative(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+     617             :   bool callbackReferenceService(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+     618             :   bool callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrv::Request& req, mrs_msgs::VelocityReferenceStampedSrv::Response& res);
+     619             :   bool callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrv::Request& req, mrs_msgs::TrajectoryReferenceSrv::Response& res);
+     620             :   bool callbackEmergencyReference(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+     621             : 
+     622             :   // safety callbacks
+     623             :   bool callbackHover(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     624             :   bool callbackStartTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     625             :   bool callbackStopTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     626             :   bool callbackResumeTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     627             :   bool callbackGotoTrajectoryStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     628             :   bool callbackEHover(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     629             :   bool callbackFailsafe(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     630             :   bool callbackFailsafeEscalating(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     631             :   bool callbackEland(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     632             :   bool callbackParachute([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     633             :   bool callbackSetMinZ(mrs_msgs::Float64StampedSrv::Request& req, mrs_msgs::Float64StampedSrv::Response& res);
+     634             :   bool callbackToggleOutput(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     635             :   bool callbackArm(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     636             :   bool callbackEnableCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     637             :   bool callbackEnableBumper(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     638             :   bool callbackUseSafetyArea(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     639             : 
+     640             :   bool callbackGetMinZ(mrs_msgs::GetFloat64::Request& req, mrs_msgs::GetFloat64::Response& res);
+     641             : 
+     642             :   bool callbackValidateReference(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res);
+     643             :   bool callbackValidateReference2d(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res);
+     644             :   bool callbackValidateReferenceList(mrs_msgs::ValidateReferenceList::Request& req, mrs_msgs::ValidateReferenceList::Response& res);
+     645             : 
+     646             :   // transformation callbacks
+     647             :   bool callbackTransformReference(mrs_msgs::TransformReferenceSrv::Request& req, mrs_msgs::TransformReferenceSrv::Response& res);
+     648             :   bool callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res);
+     649             :   bool callbackTransformVector3(mrs_msgs::TransformVector3Srv::Request& req, mrs_msgs::TransformVector3Srv::Response& res);
+     650             : 
+     651             :   // | ----------------------- constraints ---------------------- |
+     652             : 
+     653             :   // sets constraints to all trackers
+     654             :   bool callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrv::Request& req, mrs_msgs::DynamicsConstraintsSrv::Response& res);
+     655             : 
+     656             :   // constraints management
+     657             :   bool              got_constraints_ = false;
+     658             :   std::mutex        mutex_constraints_;
+     659             :   void              setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     660             :   void              setConstraintsToTrackers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     661             :   void              setConstraintsToControllers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     662             :   std::atomic<bool> constraints_being_enforced_ = false;
+     663             : 
+     664             :   std::optional<mrs_msgs::DynamicsConstraintsSrvRequest> enforceControllersConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+     665             : 
+     666             :   mrs_msgs::DynamicsConstraintsSrvRequest current_constraints_;
+     667             :   mrs_msgs::DynamicsConstraintsSrvRequest sanitized_constraints_;
+     668             : 
+     669             :   // | ------------------ emergency triggered? ------------------ |
+     670             : 
+     671             :   std::atomic<bool> failsafe_triggered_ = false;
+     672             :   std::atomic<bool> eland_triggered_    = false;
+     673             : 
+     674             :   // | ------------------------- timers ------------------------- |
+     675             : 
+     676             :   // timer for regular status publishing
+     677             :   ros::Timer timer_status_;
+     678             :   void       timerStatus(const ros::TimerEvent& event);
+     679             : 
+     680             :   // timer for issuing the failsafe landing
+     681             :   ros::Timer timer_failsafe_;
+     682             :   void       timerFailsafe(const ros::TimerEvent& event);
+     683             : 
+     684             :   // oneshot timer for running controllers and trackers
+     685             :   void              asyncControl(void);
+     686             :   std::atomic<bool> running_async_control_ = false;
+     687             :   std::future<void> async_control_result_;
+     688             : 
+     689             :   // timer for issuing emergancy landing
+     690             :   ros::Timer timer_eland_;
+     691             :   void       timerEland(const ros::TimerEvent& event);
+     692             : 
+     693             :   // timer for regular checking of controller errors
+     694             :   ros::Timer        timer_safety_;
+     695             :   void              timerSafety(const ros::TimerEvent& event);
+     696             :   std::atomic<bool> running_safety_timer_        = false;
+     697             :   std::atomic<bool> odometry_switch_in_progress_ = false;
+     698             : 
+     699             :   // timer for issuing the pirouette
+     700             :   ros::Timer timer_pirouette_;
+     701             :   void       timerPirouette(const ros::TimerEvent& event);
+     702             : 
+     703             :   // | --------------------- obstacle bumper -------------------- |
+     704             : 
+     705             :   // bumper timer
+     706             :   ros::Timer timer_bumper_;
+     707             :   void       timerBumper(const ros::TimerEvent& event);
+     708             : 
+     709             :   // bumper subscriber
+     710             :   mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors> sh_bumper_;
+     711             : 
+     712             :   bool        _bumper_switch_tracker_    = false;
+     713             :   bool        _bumper_switch_controller_ = false;
+     714             :   std::string _bumper_tracker_name_;
+     715             :   std::string _bumper_controller_name_;
+     716             :   std::string bumper_previous_tracker_;
+     717             :   std::string bumper_previous_controller_;
+     718             : 
+     719             :   std::atomic<bool> bumper_enabled_   = false;
+     720             :   std::atomic<bool> bumper_repulsing_ = false;
+     721             : 
+     722             :   bool _bumper_horizontal_derive_from_dynamics_;
+     723             :   bool _bumper_vertical_derive_from_dynamics_;
+     724             : 
+     725             :   double _bumper_horizontal_distance_ = 0;
+     726             :   double _bumper_vertical_distance_   = 0;
+     727             : 
+     728             :   double _bumper_horizontal_overshoot_ = 0;
+     729             :   double _bumper_vertical_overshoot_   = 0;
+     730             : 
+     731             :   int  bumperGetSectorId(const double& x, const double& y, const double& z);
+     732             :   void bumperPushFromObstacle(void);
+     733             : 
+     734             :   // | --------------- safety checks and failsafes -------------- |
+     735             : 
+     736             :   // escalating failsafe (eland -> failsafe -> disarm)
+     737             :   bool                       _service_escalating_failsafe_enabled_ = false;
+     738             :   bool                       _rc_escalating_failsafe_enabled_      = false;
+     739             :   double                     _escalating_failsafe_timeout_         = 0;
+     740             :   ros::Time                  escalating_failsafe_time_;
+     741             :   bool                       _escalating_failsafe_ehover_   = false;
+     742             :   bool                       _escalating_failsafe_eland_    = false;
+     743             :   bool                       _escalating_failsafe_failsafe_ = false;
+     744             :   double                     _rc_escalating_failsafe_threshold_;
+     745             :   int                        _rc_escalating_failsafe_channel_  = 0;
+     746             :   bool                       rc_escalating_failsafe_triggered_ = false;
+     747             :   EscalatingFailsafeStates_t state_escalating_failsafe_        = ESC_NONE_STATE;
+     748             : 
+     749             :   std::string _tracker_error_action_;
+     750             : 
+     751             :   // emergancy landing state machine
+     752             :   LandingStates_t current_state_landing_  = IDLE_STATE;
+     753             :   LandingStates_t previous_state_landing_ = IDLE_STATE;
+     754             :   std::mutex      mutex_landing_state_machine_;
+     755             :   void            changeLandingState(LandingStates_t new_state);
+     756             :   double          _uav_mass_ = 0;
+     757             :   double          _elanding_cutoff_mass_factor_;
+     758             :   double          _elanding_cutoff_timeout_;
+     759             :   double          landing_uav_mass_ = 0;
+     760             : 
+     761             :   // initial body disturbance loaded from params
+     762             :   double _initial_body_disturbance_x_ = 0;
+     763             :   double _initial_body_disturbance_y_ = 0;
+     764             : 
+     765             :   // profiling
+     766             :   mrs_lib::Profiler profiler_;
+     767             :   bool              _profiler_enabled_ = false;
+     768             : 
+     769             :   // diagnostics publishing
+     770             :   void       publishDiagnostics(void);
+     771             :   std::mutex mutex_diagnostics_;
+     772             : 
+     773             :   void                                             ungripSrv(void);
+     774             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_ungrip_;
+     775             : 
+     776             :   bool isFlyingNormally(void);
+     777             : 
+     778             :   // | ------------------------ pirouette ----------------------- |
+     779             : 
+     780             :   bool       _pirouette_enabled_ = false;
+     781             :   double     _pirouette_speed_;
+     782             :   double     _pirouette_timer_rate_;
+     783             :   std::mutex mutex_pirouette_;
+     784             :   double     pirouette_initial_heading_;
+     785             :   double     pirouette_iterator_;
+     786             :   bool       callbackPirouette(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     787             : 
+     788             :   // | -------------------- joystick control -------------------- |
+     789             : 
+     790             :   mrs_lib::SubscribeHandler<sensor_msgs::Joy> sh_joystick_;
+     791             : 
+     792             :   void callbackJoystick(const sensor_msgs::Joy::ConstPtr msg);
+     793             :   bool callbackUseJoystick([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+     794             : 
+     795             :   // joystick buttons mappings
+     796             :   int _channel_A_, _channel_B_, _channel_X_, _channel_Y_, _channel_start_, _channel_back_, _channel_LT_, _channel_RT_, _channel_L_joy_, _channel_R_joy_;
+     797             : 
+     798             :   // channel numbers and channel multipliers
+     799             :   int    _channel_pitch_, _channel_roll_, _channel_heading_, _channel_throttle_;
+     800             :   double _channel_mult_pitch_, _channel_mult_roll_, _channel_mult_heading_, _channel_mult_throttle_;
+     801             : 
+     802             :   ros::Timer timer_joystick_;
+     803             :   void       timerJoystick(const ros::TimerEvent& event);
+     804             :   double     _joystick_timer_rate_ = 0;
+     805             : 
+     806             :   double _joystick_carrot_distance_ = 0;
+     807             : 
+     808             :   ros::Time joystick_start_press_time_;
+     809             :   bool      joystick_start_pressed_ = false;
+     810             : 
+     811             :   ros::Time joystick_back_press_time_;
+     812             :   bool      joystick_back_pressed_ = false;
+     813             :   bool      joystick_goto_enabled_ = false;
+     814             : 
+     815             :   bool      joystick_failsafe_pressed_ = false;
+     816             :   ros::Time joystick_failsafe_press_time_;
+     817             : 
+     818             :   bool      joystick_eland_pressed_ = false;
+     819             :   ros::Time joystick_eland_press_time_;
+     820             : 
+     821             :   // | ------------------- RC joystick control ------------------ |
+     822             : 
+     823             :   // listening to the RC channels as told by pixhawk
+     824             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels> sh_hw_api_rc_;
+     825             : 
+     826             :   // the RC channel mapping of the main 4 control signals
+     827             :   double _rc_channel_pitch_, _rc_channel_roll_, _rc_channel_heading_, _rc_channel_throttle_;
+     828             : 
+     829             :   bool              _rc_goto_enabled_               = false;
+     830             :   std::atomic<bool> rc_goto_active_                 = false;
+     831             :   double            rc_joystick_channel_last_value_ = 0.5;
+     832             :   bool              rc_joystick_channel_was_low_    = false;
+     833             :   int               _rc_joystick_channel_           = 0;
+     834             : 
+     835             :   double _rc_horizontal_speed_ = 0;
+     836             :   double _rc_vertical_speed_   = 0;
+     837             :   double _rc_heading_rate_     = 0;
+     838             : 
+     839             :   // | ------------------- trajectory loading ------------------- |
+     840             : 
+     841             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>        pub_debug_original_trajectory_poses_;
+     842             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray> pub_debug_original_trajectory_markers_;
+     843             : 
+     844             :   // | --------------------- other routines --------------------- |
+     845             : 
+     846             :   // this is called to update the trackers and to receive position control command from the active one
+     847             :   void updateTrackers(void);
+     848             : 
+     849             :   // this is called to update the controllers and to receive attitude control command from the active one
+     850             :   void updateControllers(const mrs_msgs::UavState& uav_state);
+     851             : 
+     852             :   // sets the reference to the active tracker
+     853             :   std::tuple<bool, std::string> setReference(const mrs_msgs::ReferenceStamped reference_in);
+     854             : 
+     855             :   // sets the velocity reference to the active tracker
+     856             :   std::tuple<bool, std::string> setVelocityReference(const mrs_msgs::VelocityReferenceStamped& reference_in);
+     857             : 
+     858             :   // sets the reference trajectory to the active tracker
+     859             :   std::tuple<bool, std::string, bool, std::vector<std::string>, std::vector<bool>, std::vector<std::string>> setTrajectoryReference(
+     860             :       const mrs_msgs::TrajectoryReference trajectory_in);
+     861             : 
+     862             :   // publishes
+     863             :   void publish(void);
+     864             : 
+     865             :   bool loadConfigFile(const std::string& file_path, const std::string ns);
+     866             : 
+     867             :   double getMass(void);
+     868             : 
+     869             :   // publishes rviz-visualizable control reference
+     870             :   void publishControlReferenceOdom(const std::optional<mrs_msgs::TrackerCommand>& tracker_command, const Controller::ControlOutput& control_output);
+     871             : 
+     872             :   void initializeControlOutput(void);
+     873             : 
+     874             :   // tell the mrs_odometry to disable its callbacks
+     875             :   void odometryCallbacksSrv(const bool input);
+     876             : 
+     877             :   mrs_msgs::ReferenceStamped velocityReferenceToReference(const mrs_msgs::VelocityReferenceStamped& vel_reference);
+     878             : 
+     879             :   void                          setCallbacks(bool in);
+     880             :   bool                          isOffboard(void);
+     881             :   bool                          elandSrv(void);
+     882             :   std::tuple<bool, std::string> arming(const bool input);
+     883             : 
+     884             :   // safety functions impl
+     885             :   std::tuple<bool, std::string> ehover(void);
+     886             :   std::tuple<bool, std::string> hover(void);
+     887             :   std::tuple<bool, std::string> startTrajectoryTracking(void);
+     888             :   std::tuple<bool, std::string> stopTrajectoryTracking(void);
+     889             :   std::tuple<bool, std::string> resumeTrajectoryTracking(void);
+     890             :   std::tuple<bool, std::string> gotoTrajectoryStart(void);
+     891             :   std::tuple<bool, std::string> eland(void);
+     892             :   std::tuple<bool, std::string> failsafe(void);
+     893             :   std::tuple<bool, std::string> escalatingFailsafe(void);
+     894             : 
+     895             :   EscalatingFailsafeStates_t getNextEscFailsafeState(void);
+     896             : };
+     897             : 
+     898             : //}
+     899             : 
+     900             : /* //{ onInit() */
+     901             : 
+     902          82 : void ControlManager::onInit() {
+     903          82 :   preinitialize();
+     904          82 : }
+     905             : 
+     906             : //}
+     907             : 
+     908             : /* preinitialize() //{ */
+     909             : 
+     910          82 : void ControlManager::preinitialize(void) {
+     911             : 
+     912          82 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     913             : 
+     914          82 :   ros::Time::waitForValid();
+     915             : 
+     916          82 :   mrs_lib::SubscribeHandlerOptions shopts;
+     917          82 :   shopts.nh                 = nh_;
+     918          82 :   shopts.node_name          = "ControlManager";
+     919          82 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     920          82 :   shopts.threadsafe         = true;
+     921          82 :   shopts.autostart          = true;
+     922          82 :   shopts.queue_size         = 10;
+     923          82 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     924             : 
+     925          82 :   sh_hw_api_capabilities_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+     926             : 
+     927          82 :   timer_hw_api_capabilities_ = nh_.createTimer(ros::Rate(1.0), &ControlManager::timerHwApiCapabilities, this);
+     928          82 : }
+     929             : 
+     930             : //}
+     931             : 
+     932             : /* initialize() //{ */
+     933             : 
+     934          82 : void ControlManager::initialize(void) {
+     935             : 
+     936          82 :   joystick_start_press_time_      = ros::Time(0);
+     937          82 :   joystick_failsafe_press_time_   = ros::Time(0);
+     938          82 :   joystick_eland_press_time_      = ros::Time(0);
+     939          82 :   escalating_failsafe_time_       = ros::Time(0);
+     940          82 :   controller_tracker_switch_time_ = ros::Time(0);
+     941             : 
+     942          82 :   ROS_INFO("[ControlManager]: initializing");
+     943             : 
+     944             :   // --------------------------------------------------------------
+     945             :   // |         common handler for trackers and controllers        |
+     946             :   // --------------------------------------------------------------
+     947             : 
+     948          82 :   common_handlers_ = std::make_shared<mrs_uav_managers::control_manager::CommonHandlers_t>();
+     949             : 
+     950             :   // --------------------------------------------------------------
+     951             :   // |                           params                           |
+     952             :   // --------------------------------------------------------------
+     953             : 
+     954         164 :   mrs_lib::ParamLoader param_loader(nh_, "ControlManager");
+     955             : 
+     956          82 :   param_loader.loadParam("custom_config", _custom_config_);
+     957          82 :   param_loader.loadParam("platform_config", _platform_config_);
+     958          82 :   param_loader.loadParam("world_config", _world_config_);
+     959          82 :   param_loader.loadParam("network_config", _network_config_);
+     960             : 
+     961          82 :   if (_custom_config_ != "") {
+     962          82 :     param_loader.addYamlFile(_custom_config_);
+     963             :   }
+     964             : 
+     965          82 :   if (_platform_config_ != "") {
+     966          82 :     param_loader.addYamlFile(_platform_config_);
+     967             :   }
+     968             : 
+     969          82 :   if (_world_config_ != "") {
+     970          82 :     param_loader.addYamlFile(_world_config_);
+     971             :   }
+     972             : 
+     973          82 :   if (_network_config_ != "") {
+     974          82 :     param_loader.addYamlFile(_network_config_);
+     975             :   }
+     976             : 
+     977          82 :   param_loader.addYamlFileFromParam("private_config");
+     978          82 :   param_loader.addYamlFileFromParam("public_config");
+     979          82 :   param_loader.addYamlFileFromParam("private_trackers");
+     980          82 :   param_loader.addYamlFileFromParam("private_controllers");
+     981          82 :   param_loader.addYamlFileFromParam("public_controllers");
+     982             : 
+     983             :   // params passed from the launch file are not prefixed
+     984          82 :   param_loader.loadParam("uav_name", _uav_name_);
+     985          82 :   param_loader.loadParam("body_frame", _body_frame_);
+     986          82 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     987          82 :   param_loader.loadParam("uav_mass", _uav_mass_);
+     988          82 :   param_loader.loadParam("body_disturbance_x", _initial_body_disturbance_x_);
+     989          82 :   param_loader.loadParam("body_disturbance_y", _initial_body_disturbance_y_);
+     990          82 :   param_loader.loadParam("g", common_handlers_->g);
+     991             : 
+     992             :   // motor params are also not prefixed, since they are common to more nodes
+     993          82 :   param_loader.loadParam("motor_params/a", common_handlers_->throttle_model.A);
+     994          82 :   param_loader.loadParam("motor_params/b", common_handlers_->throttle_model.B);
+     995          82 :   param_loader.loadParam("motor_params/n_motors", common_handlers_->throttle_model.n_motors);
+     996             : 
+     997             :   // | ----------------------- safety area ---------------------- |
+     998             : 
+     999             :   bool use_safety_area;
+    1000          82 :   param_loader.loadParam("safety_area/enabled", use_safety_area);
+    1001          82 :   use_safety_area_ = use_safety_area;
+    1002             : 
+    1003          82 :   param_loader.loadParam("safety_area/horizontal/frame_name", _safety_area_horizontal_frame_);
+    1004             : 
+    1005          82 :   param_loader.loadParam("safety_area/vertical/frame_name", _safety_area_vertical_frame_);
+    1006          82 :   param_loader.loadParam("safety_area/vertical/max_z", _safety_area_max_z_);
+    1007             : 
+    1008             :   {
+    1009             :     double temp;
+    1010          82 :     param_loader.loadParam("safety_area/vertical/min_z", temp);
+    1011             : 
+    1012          82 :     _safety_area_min_z_ = temp;
+    1013             :   }
+    1014             : 
+    1015          82 :   if (use_safety_area_) {
+    1016             : 
+    1017         186 :     Eigen::MatrixXd border_points = param_loader.loadMatrixDynamic2("safety_area/horizontal/points", -1, 2);
+    1018             : 
+    1019             :     try {
+    1020             : 
+    1021         124 :       std::vector<Eigen::MatrixXd> polygon_obstacle_points;
+    1022          62 :       std::vector<Eigen::MatrixXd> point_obstacle_points;
+    1023             : 
+    1024          62 :       safety_zone_ = std::make_unique<mrs_lib::SafetyZone>(border_points);
+    1025             :     }
+    1026             : 
+    1027           0 :     catch (mrs_lib::SafetyZone::BorderError& e) {
+    1028           0 :       ROS_ERROR("[ControlManager]: SafetyArea: wrong configruation for the safety zone border polygon");
+    1029           0 :       ros::shutdown();
+    1030             :     }
+    1031           0 :     catch (...) {
+    1032           0 :       ROS_ERROR("[ControlManager]: SafetyArea: unhandled exception!");
+    1033           0 :       ros::shutdown();
+    1034             :     }
+    1035             : 
+    1036          62 :     ROS_INFO("[ControlManager]: safety area initialized");
+    1037             :   }
+    1038             : 
+    1039          82 :   param_loader.setPrefix("mrs_uav_managers/control_manager/");
+    1040             : 
+    1041          82 :   param_loader.loadParam("state_input", _state_input_);
+    1042             : 
+    1043          82 :   if (!(_state_input_ == INPUT_UAV_STATE || _state_input_ == INPUT_ODOMETRY)) {
+    1044           0 :     ROS_ERROR("[ControlManager]: the state_input parameter has to be in {0, 1}");
+    1045           0 :     ros::shutdown();
+    1046             :   }
+    1047             : 
+    1048          82 :   param_loader.loadParam("safety/min_throttle_null_tracker", _min_throttle_null_tracker_);
+    1049          82 :   param_loader.loadParam("safety/ehover_tracker", _ehover_tracker_name_);
+    1050          82 :   param_loader.loadParam("safety/failsafe_controller", _failsafe_controller_name_);
+    1051             : 
+    1052          82 :   param_loader.loadParam("safety/eland/controller", _eland_controller_name_);
+    1053          82 :   param_loader.loadParam("safety/eland/cutoff_mass_factor", _elanding_cutoff_mass_factor_);
+    1054          82 :   param_loader.loadParam("safety/eland/cutoff_timeout", _elanding_cutoff_timeout_);
+    1055          82 :   param_loader.loadParam("safety/eland/timer_rate", _elanding_timer_rate_);
+    1056          82 :   param_loader.loadParam("safety/eland/disarm", _eland_disarm_enabled_);
+    1057             : 
+    1058          82 :   param_loader.loadParam("safety/escalating_failsafe/service/enabled", _service_escalating_failsafe_enabled_);
+    1059          82 :   param_loader.loadParam("safety/escalating_failsafe/rc/enabled", _rc_escalating_failsafe_enabled_);
+    1060          82 :   param_loader.loadParam("safety/escalating_failsafe/rc/channel_number", _rc_escalating_failsafe_channel_);
+    1061          82 :   param_loader.loadParam("safety/escalating_failsafe/rc/threshold", _rc_escalating_failsafe_threshold_);
+    1062          82 :   param_loader.loadParam("safety/escalating_failsafe/timeout", _escalating_failsafe_timeout_);
+    1063          82 :   param_loader.loadParam("safety/escalating_failsafe/ehover", _escalating_failsafe_ehover_);
+    1064          82 :   param_loader.loadParam("safety/escalating_failsafe/eland", _escalating_failsafe_eland_);
+    1065          82 :   param_loader.loadParam("safety/escalating_failsafe/failsafe", _escalating_failsafe_failsafe_);
+    1066             : 
+    1067          82 :   param_loader.loadParam("safety/tilt_limit/eland/enabled", _tilt_limit_eland_enabled_);
+    1068          82 :   param_loader.loadParam("safety/tilt_limit/eland/limit", _tilt_limit_eland_);
+    1069             : 
+    1070          82 :   _tilt_limit_eland_ = M_PI * (_tilt_limit_eland_ / 180.0);
+    1071             : 
+    1072          82 :   if (_tilt_limit_eland_enabled_ && fabs(_tilt_limit_eland_) < 1e-3) {
+    1073           0 :     ROS_ERROR("[ControlManager]: safety/tilt_limit/eland/enabled = 'TRUE' but the limit is too low");
+    1074           0 :     ros::shutdown();
+    1075             :   }
+    1076             : 
+    1077          82 :   param_loader.loadParam("safety/tilt_limit/disarm/enabled", _tilt_limit_disarm_enabled_);
+    1078          82 :   param_loader.loadParam("safety/tilt_limit/disarm/limit", _tilt_limit_disarm_);
+    1079             : 
+    1080          82 :   _tilt_limit_disarm_ = M_PI * (_tilt_limit_disarm_ / 180.0);
+    1081             : 
+    1082          82 :   if (_tilt_limit_disarm_enabled_ && fabs(_tilt_limit_disarm_) < 1e-3) {
+    1083           0 :     ROS_ERROR("[ControlManager]: safety/tilt_limit/disarm/enabled = 'TRUE' but the limit is too low");
+    1084           0 :     ros::shutdown();
+    1085             :   }
+    1086             : 
+    1087          82 :   param_loader.loadParam("safety/yaw_error_eland/enabled", _yaw_error_eland_enabled_);
+    1088          82 :   param_loader.loadParam("safety/yaw_error_eland/limit", _yaw_error_eland_);
+    1089             : 
+    1090          82 :   _yaw_error_eland_ = M_PI * (_yaw_error_eland_ / 180.0);
+    1091             : 
+    1092          82 :   if (_yaw_error_eland_enabled_ && fabs(_yaw_error_eland_) < 1e-3) {
+    1093           0 :     ROS_ERROR("[ControlManager]: safety/yaw_error_eland/enabled = 'TRUE' but the limit is too low");
+    1094           0 :     ros::shutdown();
+    1095             :   }
+    1096             : 
+    1097          82 :   param_loader.loadParam("status_timer_rate", _status_timer_rate_);
+    1098          82 :   param_loader.loadParam("safety/safety_timer_rate", _safety_timer_rate_);
+    1099          82 :   param_loader.loadParam("safety/failsafe_timer_rate", _failsafe_timer_rate_);
+    1100          82 :   param_loader.loadParam("safety/rc_emergency_handoff/enabled", _rc_emergency_handoff_);
+    1101             : 
+    1102          82 :   param_loader.loadParam("safety/odometry_max_missing_time", _uav_state_max_missing_time_);
+    1103          82 :   param_loader.loadParam("safety/odometry_innovation_eland/enabled", _odometry_innovation_check_enabled_);
+    1104             : 
+    1105          82 :   param_loader.loadParam("safety/tilt_error_disarm/enabled", _tilt_error_disarm_enabled_);
+    1106          82 :   param_loader.loadParam("safety/tilt_error_disarm/timeout", _tilt_error_disarm_timeout_);
+    1107          82 :   param_loader.loadParam("safety/tilt_error_disarm/error_threshold", _tilt_error_disarm_threshold_);
+    1108             : 
+    1109          82 :   _tilt_error_disarm_threshold_ = M_PI * (_tilt_error_disarm_threshold_ / 180.0);
+    1110             : 
+    1111          82 :   if (_tilt_error_disarm_enabled_ && fabs(_tilt_error_disarm_threshold_) < 1e-3) {
+    1112           0 :     ROS_ERROR("[ControlManager]: safety/tilt_error_disarm/enabled = 'TRUE' but the limit is too low");
+    1113           0 :     ros::shutdown();
+    1114             :   }
+    1115             : 
+    1116             :   // default constraints
+    1117             : 
+    1118          82 :   param_loader.loadParam("default_constraints/horizontal/speed", current_constraints_.constraints.horizontal_speed);
+    1119          82 :   param_loader.loadParam("default_constraints/horizontal/acceleration", current_constraints_.constraints.horizontal_acceleration);
+    1120          82 :   param_loader.loadParam("default_constraints/horizontal/jerk", current_constraints_.constraints.horizontal_jerk);
+    1121          82 :   param_loader.loadParam("default_constraints/horizontal/snap", current_constraints_.constraints.horizontal_snap);
+    1122             : 
+    1123          82 :   param_loader.loadParam("default_constraints/vertical/ascending/speed", current_constraints_.constraints.vertical_ascending_speed);
+    1124          82 :   param_loader.loadParam("default_constraints/vertical/ascending/acceleration", current_constraints_.constraints.vertical_ascending_acceleration);
+    1125          82 :   param_loader.loadParam("default_constraints/vertical/ascending/jerk", current_constraints_.constraints.vertical_ascending_jerk);
+    1126          82 :   param_loader.loadParam("default_constraints/vertical/ascending/snap", current_constraints_.constraints.vertical_ascending_snap);
+    1127             : 
+    1128          82 :   param_loader.loadParam("default_constraints/vertical/descending/speed", current_constraints_.constraints.vertical_descending_speed);
+    1129          82 :   param_loader.loadParam("default_constraints/vertical/descending/acceleration", current_constraints_.constraints.vertical_descending_acceleration);
+    1130          82 :   param_loader.loadParam("default_constraints/vertical/descending/jerk", current_constraints_.constraints.vertical_descending_jerk);
+    1131          82 :   param_loader.loadParam("default_constraints/vertical/descending/snap", current_constraints_.constraints.vertical_descending_snap);
+    1132             : 
+    1133          82 :   param_loader.loadParam("default_constraints/heading/speed", current_constraints_.constraints.heading_speed);
+    1134          82 :   param_loader.loadParam("default_constraints/heading/acceleration", current_constraints_.constraints.heading_acceleration);
+    1135          82 :   param_loader.loadParam("default_constraints/heading/jerk", current_constraints_.constraints.heading_jerk);
+    1136          82 :   param_loader.loadParam("default_constraints/heading/snap", current_constraints_.constraints.heading_snap);
+    1137             : 
+    1138          82 :   param_loader.loadParam("default_constraints/angular_speed/roll", current_constraints_.constraints.roll_rate);
+    1139          82 :   param_loader.loadParam("default_constraints/angular_speed/pitch", current_constraints_.constraints.pitch_rate);
+    1140          82 :   param_loader.loadParam("default_constraints/angular_speed/yaw", current_constraints_.constraints.yaw_rate);
+    1141             : 
+    1142          82 :   param_loader.loadParam("default_constraints/tilt", current_constraints_.constraints.tilt);
+    1143             : 
+    1144          82 :   current_constraints_.constraints.tilt = M_PI * (current_constraints_.constraints.tilt / 180.0);
+    1145             : 
+    1146             :   // joystick
+    1147             : 
+    1148          82 :   param_loader.loadParam("joystick/enabled", _joystick_enabled_);
+    1149          82 :   param_loader.loadParam("joystick/mode", _joystick_mode_);
+    1150          82 :   param_loader.loadParam("joystick/carrot_distance", _joystick_carrot_distance_);
+    1151          82 :   param_loader.loadParam("joystick/joystick_timer_rate", _joystick_timer_rate_);
+    1152          82 :   param_loader.loadParam("joystick/attitude_control/tracker", _joystick_tracker_name_);
+    1153          82 :   param_loader.loadParam("joystick/attitude_control/controller", _joystick_controller_name_);
+    1154          82 :   param_loader.loadParam("joystick/attitude_control/fallback/tracker", _joystick_fallback_tracker_name_);
+    1155          82 :   param_loader.loadParam("joystick/attitude_control/fallback/controller", _joystick_fallback_controller_name_);
+    1156             : 
+    1157          82 :   param_loader.loadParam("joystick/channels/A", _channel_A_);
+    1158          82 :   param_loader.loadParam("joystick/channels/B", _channel_B_);
+    1159          82 :   param_loader.loadParam("joystick/channels/X", _channel_X_);
+    1160          82 :   param_loader.loadParam("joystick/channels/Y", _channel_Y_);
+    1161          82 :   param_loader.loadParam("joystick/channels/start", _channel_start_);
+    1162          82 :   param_loader.loadParam("joystick/channels/back", _channel_back_);
+    1163          82 :   param_loader.loadParam("joystick/channels/LT", _channel_LT_);
+    1164          82 :   param_loader.loadParam("joystick/channels/RT", _channel_RT_);
+    1165          82 :   param_loader.loadParam("joystick/channels/L_joy", _channel_L_joy_);
+    1166          82 :   param_loader.loadParam("joystick/channels/R_joy", _channel_R_joy_);
+    1167             : 
+    1168             :   // load channels
+    1169          82 :   param_loader.loadParam("joystick/channels/pitch", _channel_pitch_);
+    1170          82 :   param_loader.loadParam("joystick/channels/roll", _channel_roll_);
+    1171          82 :   param_loader.loadParam("joystick/channels/heading", _channel_heading_);
+    1172          82 :   param_loader.loadParam("joystick/channels/throttle", _channel_throttle_);
+    1173             : 
+    1174             :   // load channel multipliers
+    1175          82 :   param_loader.loadParam("joystick/channel_multipliers/pitch", _channel_mult_pitch_);
+    1176          82 :   param_loader.loadParam("joystick/channel_multipliers/roll", _channel_mult_roll_);
+    1177          82 :   param_loader.loadParam("joystick/channel_multipliers/heading", _channel_mult_heading_);
+    1178          82 :   param_loader.loadParam("joystick/channel_multipliers/throttle", _channel_mult_throttle_);
+    1179             : 
+    1180             :   bool bumper_enabled;
+    1181          82 :   param_loader.loadParam("obstacle_bumper/enabled", bumper_enabled);
+    1182          82 :   bumper_enabled_ = bumper_enabled;
+    1183             : 
+    1184          82 :   param_loader.loadParam("obstacle_bumper/switch_tracker", _bumper_switch_tracker_);
+    1185          82 :   param_loader.loadParam("obstacle_bumper/switch_controller", _bumper_switch_controller_);
+    1186          82 :   param_loader.loadParam("obstacle_bumper/tracker", _bumper_tracker_name_);
+    1187          82 :   param_loader.loadParam("obstacle_bumper/controller", _bumper_controller_name_);
+    1188          82 :   param_loader.loadParam("obstacle_bumper/timer_rate", _bumper_timer_rate_);
+    1189             : 
+    1190          82 :   param_loader.loadParam("obstacle_bumper/horizontal/min_distance_to_obstacle", _bumper_horizontal_distance_);
+    1191          82 :   param_loader.loadParam("obstacle_bumper/horizontal/derived_from_dynamics", _bumper_horizontal_derive_from_dynamics_);
+    1192             : 
+    1193          82 :   param_loader.loadParam("obstacle_bumper/vertical/min_distance_to_obstacle", _bumper_vertical_distance_);
+    1194          82 :   param_loader.loadParam("obstacle_bumper/vertical/derived_from_dynamics", _bumper_vertical_derive_from_dynamics_);
+    1195             : 
+    1196          82 :   param_loader.loadParam("obstacle_bumper/horizontal/overshoot", _bumper_horizontal_overshoot_);
+    1197          82 :   param_loader.loadParam("obstacle_bumper/vertical/overshoot", _bumper_vertical_overshoot_);
+    1198             : 
+    1199          82 :   param_loader.loadParam("safety/tracker_error_action", _tracker_error_action_);
+    1200             : 
+    1201          82 :   param_loader.loadParam("trajectory_tracking/snap_to_safety_area", _snap_trajectory_to_safety_area_);
+    1202             : 
+    1203             :   // check the values of tracker error action
+    1204          82 :   if (_tracker_error_action_ != ELAND_STR && _tracker_error_action_ != EHOVER_STR) {
+    1205           0 :     ROS_ERROR("[ControlManager]: the tracker_error_action parameter (%s) is not correct, requires {%s, %s}", _tracker_error_action_.c_str(), ELAND_STR,
+    1206             :               EHOVER_STR);
+    1207           0 :     ros::shutdown();
+    1208             :   }
+    1209             : 
+    1210          82 :   param_loader.loadParam("rc_joystick/enabled", _rc_goto_enabled_);
+    1211          82 :   param_loader.loadParam("rc_joystick/channel_number", _rc_joystick_channel_);
+    1212          82 :   param_loader.loadParam("rc_joystick/horizontal_speed", _rc_horizontal_speed_);
+    1213          82 :   param_loader.loadParam("rc_joystick/vertical_speed", _rc_vertical_speed_);
+    1214          82 :   param_loader.loadParam("rc_joystick/heading_rate", _rc_heading_rate_);
+    1215             : 
+    1216          82 :   param_loader.loadParam("rc_joystick/channels/pitch", _rc_channel_pitch_);
+    1217          82 :   param_loader.loadParam("rc_joystick/channels/roll", _rc_channel_roll_);
+    1218          82 :   param_loader.loadParam("rc_joystick/channels/heading", _rc_channel_heading_);
+    1219          82 :   param_loader.loadParam("rc_joystick/channels/throttle", _rc_channel_throttle_);
+    1220             : 
+    1221          82 :   param_loader.loadParam("pirouette/speed", _pirouette_speed_);
+    1222          82 :   param_loader.loadParam("pirouette/timer_rate", _pirouette_timer_rate_);
+    1223             : 
+    1224          82 :   param_loader.loadParam("safety/parachute/enabled", _parachute_enabled_);
+    1225             : 
+    1226             :   // --------------------------------------------------------------
+    1227             :   // |             initialize the last control output             |
+    1228             :   // --------------------------------------------------------------
+    1229             : 
+    1230          82 :   initializeControlOutput();
+    1231             : 
+    1232             :   // | --------------------- tf transformer --------------------- |
+    1233             : 
+    1234          82 :   transformer_ = std::make_shared<mrs_lib::Transformer>(nh_, "ControlManager");
+    1235          82 :   transformer_->setDefaultPrefix(_uav_name_);
+    1236          82 :   transformer_->retryLookupNewest(true);
+    1237             : 
+    1238             :   // | ------------------- scope timer logger ------------------- |
+    1239             : 
+    1240          82 :   param_loader.loadParam("scope_timer/enabled", scope_timer_enabled_);
+    1241         246 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+    1242          82 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+    1243             : 
+    1244             :   // bind transformer to trackers and controllers for use
+    1245          82 :   common_handlers_->transformer = transformer_;
+    1246             : 
+    1247             :   // bind scope timer to trackers and controllers for use
+    1248          82 :   common_handlers_->scope_timer.enabled = scope_timer_enabled_;
+    1249          82 :   common_handlers_->scope_timer.logger  = scope_timer_logger_;
+    1250             : 
+    1251          82 :   common_handlers_->safety_area.use_safety_area       = use_safety_area_;
+    1252          82 :   common_handlers_->safety_area.isPointInSafetyArea2d = boost::bind(&ControlManager::isPointInSafetyArea2d, this, _1);
+    1253          82 :   common_handlers_->safety_area.isPointInSafetyArea3d = boost::bind(&ControlManager::isPointInSafetyArea3d, this, _1);
+    1254          82 :   common_handlers_->safety_area.getMinZ               = boost::bind(&ControlManager::getMinZ, this, _1);
+    1255          82 :   common_handlers_->safety_area.getMaxZ               = boost::bind(&ControlManager::getMaxZ, this, _1);
+    1256             : 
+    1257          82 :   common_handlers_->getMass = boost::bind(&ControlManager::getMass, this);
+    1258             : 
+    1259          82 :   common_handlers_->detailed_model_params = loadDetailedUavModelParams(nh_, "ControlManager", _platform_config_, _custom_config_);
+    1260             : 
+    1261          82 :   common_handlers_->control_output_modalities = _hw_api_inputs_;
+    1262             : 
+    1263          82 :   common_handlers_->uav_name = _uav_name_;
+    1264             : 
+    1265          82 :   common_handlers_->parent_nh = nh_;
+    1266             : 
+    1267             :   // --------------------------------------------------------------
+    1268             :   // |                        load trackers                       |
+    1269             :   // --------------------------------------------------------------
+    1270             : 
+    1271         164 :   std::vector<std::string> custom_trackers;
+    1272             : 
+    1273          82 :   param_loader.loadParam("mrs_trackers", _tracker_names_);
+    1274          82 :   param_loader.loadParam("trackers", custom_trackers);
+    1275             : 
+    1276          82 :   if (!custom_trackers.empty()) {
+    1277           1 :     _tracker_names_.insert(_tracker_names_.end(), custom_trackers.begin(), custom_trackers.end());
+    1278             :   }
+    1279             : 
+    1280          82 :   param_loader.loadParam("null_tracker", _null_tracker_name_);
+    1281          82 :   param_loader.loadParam("landing_takeoff_tracker", _landoff_tracker_name_);
+    1282             : 
+    1283          82 :   tracker_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::Tracker>>("mrs_uav_managers", "mrs_uav_managers::Tracker");
+    1284             : 
+    1285         575 :   for (int i = 0; i < int(_tracker_names_.size()); i++) {
+    1286             : 
+    1287         986 :     std::string tracker_name = _tracker_names_[i];
+    1288             : 
+    1289             :     // load the controller parameters
+    1290         986 :     std::string address;
+    1291         986 :     std::string name_space;
+    1292             :     bool        human_switchable;
+    1293             : 
+    1294         493 :     param_loader.loadParam(tracker_name + "/address", address);
+    1295         493 :     param_loader.loadParam(tracker_name + "/namespace", name_space);
+    1296         493 :     param_loader.loadParam(tracker_name + "/human_switchable", human_switchable, false);
+    1297             : 
+    1298        1479 :     TrackerParams new_tracker(address, name_space, human_switchable);
+    1299         493 :     trackers_.insert(std::pair<std::string, TrackerParams>(tracker_name, new_tracker));
+    1300             : 
+    1301             :     try {
+    1302         493 :       ROS_INFO("[ControlManager]: loading the tracker '%s'", new_tracker.address.c_str());
+    1303         493 :       tracker_list_.push_back(tracker_loader_->createInstance(new_tracker.address.c_str()));
+    1304             :     }
+    1305           0 :     catch (pluginlib::CreateClassException& ex1) {
+    1306           0 :       ROS_ERROR("[ControlManager]: CreateClassException for the tracker '%s'", new_tracker.address.c_str());
+    1307           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex1.what());
+    1308           0 :       ros::shutdown();
+    1309             :     }
+    1310           0 :     catch (pluginlib::PluginlibException& ex) {
+    1311           0 :       ROS_ERROR("[ControlManager]: PluginlibException for the tracker '%s'", new_tracker.address.c_str());
+    1312           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex.what());
+    1313           0 :       ros::shutdown();
+    1314             :     }
+    1315             :   }
+    1316             : 
+    1317          82 :   ROS_INFO("[ControlManager]: trackers were loaded");
+    1318             : 
+    1319         575 :   for (int i = 0; i < int(tracker_list_.size()); i++) {
+    1320             : 
+    1321         493 :     std::map<std::string, TrackerParams>::iterator it;
+    1322         493 :     it = trackers_.find(_tracker_names_[i]);
+    1323             : 
+    1324             :     // create private handlers
+    1325             :     std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers =
+    1326         986 :         std::make_shared<mrs_uav_managers::control_manager::PrivateHandlers_t>();
+    1327             : 
+    1328         493 :     private_handlers->loadConfigFile = boost::bind(&ControlManager::loadConfigFile, this, _1, it->second.name_space);
+    1329         493 :     private_handlers->name_space     = it->second.name_space;
+    1330         493 :     private_handlers->runtime_name   = _tracker_names_[i];
+    1331         493 :     private_handlers->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, it->second.name_space), _tracker_names_[i]);
+    1332             : 
+    1333         493 :     if (_custom_config_ != "") {
+    1334         493 :       private_handlers->param_loader->addYamlFile(_custom_config_);
+    1335             :     }
+    1336             : 
+    1337         493 :     if (_platform_config_ != "") {
+    1338         493 :       private_handlers->param_loader->addYamlFile(_platform_config_);
+    1339             :     }
+    1340             : 
+    1341         493 :     if (_world_config_ != "") {
+    1342         493 :       private_handlers->param_loader->addYamlFile(_world_config_);
+    1343             :     }
+    1344             : 
+    1345         493 :     if (_network_config_ != "") {
+    1346         493 :       private_handlers->param_loader->addYamlFile(_network_config_);
+    1347             :     }
+    1348             : 
+    1349         493 :     bool success = false;
+    1350             : 
+    1351             :     try {
+    1352         493 :       ROS_INFO("[ControlManager]: initializing the tracker '%s'", it->second.address.c_str());
+    1353         493 :       success = tracker_list_[i]->initialize(ros::NodeHandle(nh_, it->second.name_space), common_handlers_, private_handlers);
+    1354             :     }
+    1355           0 :     catch (std::runtime_error& ex) {
+    1356           0 :       ROS_ERROR("[ControlManager]: exception caught during tracker initialization: '%s'", ex.what());
+    1357             :     }
+    1358             : 
+    1359         493 :     if (!success) {
+    1360           0 :       ROS_ERROR("[ControlManager]: failed to initialize the tracker '%s'", it->second.address.c_str());
+    1361           0 :       ros::shutdown();
+    1362             :     }
+    1363             :   }
+    1364             : 
+    1365          82 :   ROS_INFO("[ControlManager]: trackers were initialized");
+    1366             : 
+    1367             :   // --------------------------------------------------------------
+    1368             :   // |           check the existance of selected trackers         |
+    1369             :   // --------------------------------------------------------------
+    1370             : 
+    1371             :   // | ------ check for the existance of the hover tracker ------ |
+    1372             : 
+    1373             :   // check if the hover_tracker is within the loaded trackers
+    1374             :   {
+    1375          82 :     auto idx = idxInVector(_ehover_tracker_name_, _tracker_names_);
+    1376             : 
+    1377          82 :     if (idx) {
+    1378          82 :       _ehover_tracker_idx_ = idx.value();
+    1379             :     } else {
+    1380           0 :       ROS_ERROR("[ControlManager]: the safety/hover_tracker (%s) is not within the loaded trackers", _ehover_tracker_name_.c_str());
+    1381           0 :       ros::shutdown();
+    1382             :     }
+    1383             :   }
+    1384             : 
+    1385             :   // | ----- check for the existence of the landoff tracker ----- |
+    1386             : 
+    1387             :   {
+    1388          82 :     auto idx = idxInVector(_landoff_tracker_name_, _tracker_names_);
+    1389             : 
+    1390          82 :     if (idx) {
+    1391          82 :       _landoff_tracker_idx_ = idx.value();
+    1392             :     } else {
+    1393           0 :       ROS_ERROR("[ControlManager]: the landoff tracker (%s) is not within the loaded trackers", _landoff_tracker_name_.c_str());
+    1394           0 :       ros::shutdown();
+    1395             :     }
+    1396             :   }
+    1397             : 
+    1398             :   // | ------- check for the existence of the null tracker ------ |
+    1399             : 
+    1400             :   {
+    1401          82 :     auto idx = idxInVector(_null_tracker_name_, _tracker_names_);
+    1402             : 
+    1403          82 :     if (idx) {
+    1404          82 :       _null_tracker_idx_ = idx.value();
+    1405             :     } else {
+    1406           0 :       ROS_ERROR("[ControlManager]: the null tracker (%s) is not within the loaded trackers", _null_tracker_name_.c_str());
+    1407           0 :       ros::shutdown();
+    1408             :     }
+    1409             :   }
+    1410             : 
+    1411             :   // --------------------------------------------------------------
+    1412             :   // |         check existance of trackers for joystick           |
+    1413             :   // --------------------------------------------------------------
+    1414             : 
+    1415          82 :   if (_joystick_enabled_) {
+    1416             : 
+    1417          82 :     auto idx = idxInVector(_joystick_tracker_name_, _tracker_names_);
+    1418             : 
+    1419          82 :     if (idx) {
+    1420          82 :       _joystick_tracker_idx_ = idx.value();
+    1421             :     } else {
+    1422           0 :       ROS_ERROR("[ControlManager]: the joystick tracker (%s) is not within the loaded trackers", _joystick_tracker_name_.c_str());
+    1423           0 :       ros::shutdown();
+    1424             :     }
+    1425             :   }
+    1426             : 
+    1427          82 :   if (_bumper_switch_tracker_) {
+    1428             : 
+    1429          82 :     auto idx = idxInVector(_bumper_tracker_name_, _tracker_names_);
+    1430             : 
+    1431          82 :     if (!idx) {
+    1432           0 :       ROS_ERROR("[ControlManager]: the bumper tracker (%s) is not within the loaded trackers", _bumper_tracker_name_.c_str());
+    1433           0 :       ros::shutdown();
+    1434             :     }
+    1435             :   }
+    1436             : 
+    1437             :   {
+    1438          82 :     auto idx = idxInVector(_joystick_fallback_tracker_name_, _tracker_names_);
+    1439             : 
+    1440          82 :     if (idx) {
+    1441          82 :       _joystick_fallback_tracker_idx_ = idx.value();
+    1442             :     } else {
+    1443           0 :       ROS_ERROR("[ControlManager]: the joystick fallback tracker (%s) is not within the loaded trackers", _joystick_fallback_tracker_name_.c_str());
+    1444           0 :       ros::shutdown();
+    1445             :     }
+    1446             :   }
+    1447             : 
+    1448             :   // --------------------------------------------------------------
+    1449             :   // |                    load the controllers                    |
+    1450             :   // --------------------------------------------------------------
+    1451             : 
+    1452         164 :   std::vector<std::string> custom_controllers;
+    1453             : 
+    1454          82 :   param_loader.loadParam("mrs_controllers", _controller_names_);
+    1455          82 :   param_loader.loadParam("controllers", custom_controllers);
+    1456             : 
+    1457          82 :   if (!custom_controllers.empty()) {
+    1458           0 :     _controller_names_.insert(_controller_names_.end(), custom_controllers.begin(), custom_controllers.end());
+    1459             :   }
+    1460             : 
+    1461          82 :   controller_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::Controller>>("mrs_uav_managers", "mrs_uav_managers::Controller");
+    1462             : 
+    1463             :   // for each controller in the list
+    1464         492 :   for (int i = 0; i < int(_controller_names_.size()); i++) {
+    1465             : 
+    1466         820 :     std::string controller_name = _controller_names_[i];
+    1467             : 
+    1468             :     // load the controller parameters
+    1469         820 :     std::string address;
+    1470         820 :     std::string name_space;
+    1471             :     double      eland_threshold, failsafe_threshold, odometry_innovation_threshold;
+    1472             :     bool        human_switchable;
+    1473         410 :     param_loader.loadParam(controller_name + "/address", address);
+    1474         410 :     param_loader.loadParam(controller_name + "/namespace", name_space);
+    1475         410 :     param_loader.loadParam(controller_name + "/eland_threshold", eland_threshold);
+    1476         410 :     param_loader.loadParam(controller_name + "/failsafe_threshold", failsafe_threshold);
+    1477         410 :     param_loader.loadParam(controller_name + "/odometry_innovation_threshold", odometry_innovation_threshold);
+    1478         410 :     param_loader.loadParam(controller_name + "/human_switchable", human_switchable, false);
+    1479             : 
+    1480             :     // check if the controller can output some of the required outputs
+    1481             :     {
+    1482             : 
+    1483         410 :       ControlOutputModalities_t outputs;
+    1484         410 :       param_loader.loadParam(controller_name + "/outputs/actuators", outputs.actuators, false);
+    1485         410 :       param_loader.loadParam(controller_name + "/outputs/control_group", outputs.control_group, false);
+    1486         410 :       param_loader.loadParam(controller_name + "/outputs/attitude_rate", outputs.attitude_rate, false);
+    1487         410 :       param_loader.loadParam(controller_name + "/outputs/attitude", outputs.attitude, false);
+    1488         410 :       param_loader.loadParam(controller_name + "/outputs/acceleration_hdg_rate", outputs.acceleration_hdg_rate, false);
+    1489         410 :       param_loader.loadParam(controller_name + "/outputs/acceleration_hdg", outputs.acceleration_hdg, false);
+    1490         410 :       param_loader.loadParam(controller_name + "/outputs/velocity_hdg_rate", outputs.velocity_hdg_rate, false);
+    1491         410 :       param_loader.loadParam(controller_name + "/outputs/velocity_hdg", outputs.velocity_hdg, false);
+    1492         410 :       param_loader.loadParam(controller_name + "/outputs/position", outputs.position, false);
+    1493             : 
+    1494         410 :       bool meets_actuators             = (_hw_api_inputs_.actuators && outputs.actuators);
+    1495         410 :       bool meets_control_group         = (_hw_api_inputs_.control_group && outputs.control_group);
+    1496         410 :       bool meets_attitude_rate         = (_hw_api_inputs_.attitude_rate && outputs.attitude_rate);
+    1497         410 :       bool meets_attitude              = (_hw_api_inputs_.attitude && outputs.attitude);
+    1498         410 :       bool meets_acceleration_hdg_rate = (_hw_api_inputs_.acceleration_hdg_rate && outputs.acceleration_hdg_rate);
+    1499         410 :       bool meets_acceleration_hdg      = (_hw_api_inputs_.acceleration_hdg && outputs.acceleration_hdg);
+    1500         410 :       bool meets_velocity_hdg_rate     = (_hw_api_inputs_.velocity_hdg_rate && outputs.velocity_hdg_rate);
+    1501         410 :       bool meets_velocity_hdg          = (_hw_api_inputs_.velocity_hdg && outputs.velocity_hdg);
+    1502         410 :       bool meets_position              = (_hw_api_inputs_.position && outputs.position);
+    1503             : 
+    1504         395 :       bool meets_requirements = meets_actuators || meets_control_group || meets_attitude_rate || meets_attitude || meets_acceleration_hdg_rate ||
+    1505         805 :                                 meets_acceleration_hdg || meets_velocity_hdg_rate || meets_velocity_hdg || meets_position;
+    1506             : 
+    1507         410 :       if (!meets_requirements) {
+    1508             : 
+    1509           0 :         ROS_ERROR("[ControlManager]: the controller '%s' does not meet the control output requirements, which are some of the following",
+    1510             :                   controller_name.c_str());
+    1511             : 
+    1512           0 :         if (_hw_api_inputs_.actuators) {
+    1513           0 :           ROS_ERROR("[ControlManager]: - actuators");
+    1514             :         }
+    1515             : 
+    1516           0 :         if (_hw_api_inputs_.control_group) {
+    1517           0 :           ROS_ERROR("[ControlManager]: - control group");
+    1518             :         }
+    1519             : 
+    1520           0 :         if (_hw_api_inputs_.attitude_rate) {
+    1521           0 :           ROS_ERROR("[ControlManager]: - attitude rate");
+    1522             :         }
+    1523             : 
+    1524           0 :         if (_hw_api_inputs_.attitude) {
+    1525           0 :           ROS_ERROR("[ControlManager]: - attitude");
+    1526             :         }
+    1527             : 
+    1528           0 :         if (_hw_api_inputs_.acceleration_hdg_rate) {
+    1529           0 :           ROS_ERROR("[ControlManager]: - acceleration+hdg rate");
+    1530             :         }
+    1531             : 
+    1532           0 :         if (_hw_api_inputs_.acceleration_hdg) {
+    1533           0 :           ROS_ERROR("[ControlManager]: - acceleration+hdg");
+    1534             :         }
+    1535             : 
+    1536           0 :         if (_hw_api_inputs_.velocity_hdg_rate) {
+    1537           0 :           ROS_ERROR("[ControlManager]: - velocity+hdg rate");
+    1538             :         }
+    1539             : 
+    1540           0 :         if (_hw_api_inputs_.velocity_hdg) {
+    1541           0 :           ROS_ERROR("[ControlManager]: - velocity+hdg");
+    1542             :         }
+    1543             : 
+    1544           0 :         if (_hw_api_inputs_.position) {
+    1545           0 :           ROS_ERROR("[ControlManager]: - position");
+    1546             :         }
+    1547             : 
+    1548           0 :         ros::shutdown();
+    1549             :       }
+    1550             : 
+    1551         410 :       if ((_hw_api_inputs_.actuators || _hw_api_inputs_.control_group) && !common_handlers_->detailed_model_params) {
+    1552           0 :         ROS_ERROR(
+    1553             :             "[ControlManager]: the HW API supports 'actuators' or 'control_group' input, but the 'detailed uav model params' were not loaded sucessfully");
+    1554           0 :         ros::shutdown();
+    1555             :       }
+    1556             :     }
+    1557             : 
+    1558             :     // | --- alter the timer rates based on the hw capabilities --- |
+    1559             : 
+    1560         410 :     CONTROL_OUTPUT lowest_output = getLowestOuput(_hw_api_inputs_);
+    1561             : 
+    1562         410 :     if (lowest_output == ACTUATORS_CMD || lowest_output == CONTROL_GROUP) {
+    1563          30 :       _safety_timer_rate_     = 200.0;
+    1564          30 :       desired_uav_state_rate_ = 250.0;
+    1565         380 :     } else if (lowest_output == ATTITUDE_RATE || lowest_output == ATTITUDE) {
+    1566         290 :       _safety_timer_rate_     = 100.0;
+    1567         290 :       desired_uav_state_rate_ = 100.0;
+    1568          90 :     } else if (lowest_output == ACCELERATION_HDG_RATE || lowest_output == ACCELERATION_HDG) {
+    1569          20 :       _safety_timer_rate_     = 30.0;
+    1570          20 :       _status_timer_rate_     = 1.0;
+    1571          20 :       desired_uav_state_rate_ = 40.0;
+    1572             : 
+    1573          20 :       if (_uav_state_max_missing_time_ < 0.2) {
+    1574           4 :         _uav_state_max_missing_time_ = 0.2;
+    1575             :       }
+    1576          70 :     } else if (lowest_output >= VELOCITY_HDG_RATE) {
+    1577          70 :       _safety_timer_rate_     = 20.0;
+    1578          70 :       _status_timer_rate_     = 1.0;
+    1579          70 :       desired_uav_state_rate_ = 20.0;
+    1580             : 
+    1581          70 :       if (_uav_state_max_missing_time_ < 1.0) {
+    1582          14 :         _uav_state_max_missing_time_ = 1.0;
+    1583             :       }
+    1584             :     }
+    1585             : 
+    1586         410 :     if (eland_threshold == 0) {
+    1587          83 :       eland_threshold = 1e6;
+    1588             :     }
+    1589             : 
+    1590         410 :     if (failsafe_threshold == 0) {
+    1591          83 :       failsafe_threshold = 1e6;
+    1592             :     }
+    1593             : 
+    1594         410 :     if (odometry_innovation_threshold == 0) {
+    1595          84 :       odometry_innovation_threshold = 1e6;
+    1596             :     }
+    1597             : 
+    1598        1230 :     ControllerParams new_controller(address, name_space, eland_threshold, failsafe_threshold, odometry_innovation_threshold, human_switchable);
+    1599         410 :     controllers_.insert(std::pair<std::string, ControllerParams>(controller_name, new_controller));
+    1600             : 
+    1601             :     try {
+    1602         410 :       ROS_INFO("[ControlManager]: loading the controller '%s'", new_controller.address.c_str());
+    1603         410 :       controller_list_.push_back(controller_loader_->createInstance(new_controller.address.c_str()));
+    1604             :     }
+    1605           0 :     catch (pluginlib::CreateClassException& ex1) {
+    1606           0 :       ROS_ERROR("[ControlManager]: CreateClassException for the controller '%s'", new_controller.address.c_str());
+    1607           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex1.what());
+    1608           0 :       ros::shutdown();
+    1609             :     }
+    1610           0 :     catch (pluginlib::PluginlibException& ex) {
+    1611           0 :       ROS_ERROR("[ControlManager]: PluginlibException for the controller '%s'", new_controller.address.c_str());
+    1612           0 :       ROS_ERROR("[ControlManager]: Error: %s", ex.what());
+    1613           0 :       ros::shutdown();
+    1614             :     }
+    1615             :   }
+    1616             : 
+    1617          82 :   ROS_INFO("[ControlManager]: controllers were loaded");
+    1618             : 
+    1619         492 :   for (int i = 0; i < int(controller_list_.size()); i++) {
+    1620             : 
+    1621         410 :     std::map<std::string, ControllerParams>::iterator it;
+    1622         410 :     it = controllers_.find(_controller_names_[i]);
+    1623             : 
+    1624             :     // create private handlers
+    1625             :     std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers =
+    1626         820 :         std::make_shared<mrs_uav_managers::control_manager::PrivateHandlers_t>();
+    1627             : 
+    1628         410 :     private_handlers->loadConfigFile = boost::bind(&ControlManager::loadConfigFile, this, _1, it->second.name_space);
+    1629         410 :     private_handlers->name_space     = it->second.name_space;
+    1630         410 :     private_handlers->runtime_name   = _controller_names_[i];
+    1631         410 :     private_handlers->param_loader   = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, it->second.name_space), _controller_names_[i]);
+    1632             : 
+    1633         410 :     if (_custom_config_ != "") {
+    1634         410 :       private_handlers->param_loader->addYamlFile(_custom_config_);
+    1635             :     }
+    1636             : 
+    1637         410 :     if (_platform_config_ != "") {
+    1638         410 :       private_handlers->param_loader->addYamlFile(_platform_config_);
+    1639             :     }
+    1640             : 
+    1641         410 :     if (_world_config_ != "") {
+    1642         410 :       private_handlers->param_loader->addYamlFile(_world_config_);
+    1643             :     }
+    1644             : 
+    1645         410 :     if (_network_config_ != "") {
+    1646         410 :       private_handlers->param_loader->addYamlFile(_network_config_);
+    1647             :     }
+    1648             : 
+    1649         410 :     bool success = false;
+    1650             : 
+    1651             :     try {
+    1652             : 
+    1653         410 :       ROS_INFO("[ControlManager]: initializing the controller '%s'", it->second.address.c_str());
+    1654         410 :       success = controller_list_[i]->initialize(ros::NodeHandle(nh_, it->second.name_space), common_handlers_, private_handlers);
+    1655             :     }
+    1656           0 :     catch (std::runtime_error& ex) {
+    1657           0 :       ROS_ERROR("[ControlManager]: exception caught during controller initialization: '%s'", ex.what());
+    1658             :     }
+    1659             : 
+    1660         410 :     if (!success) {
+    1661           0 :       ROS_ERROR("[ControlManager]: failed to initialize the controller '%s'", it->second.address.c_str());
+    1662           0 :       ros::shutdown();
+    1663             :     }
+    1664             :   }
+    1665             : 
+    1666          82 :   ROS_INFO("[ControlManager]: controllers were initialized");
+    1667             : 
+    1668             :   {
+    1669          82 :     auto idx = idxInVector(_failsafe_controller_name_, _controller_names_);
+    1670             : 
+    1671          82 :     if (idx) {
+    1672          82 :       _failsafe_controller_idx_ = idx.value();
+    1673             :     } else {
+    1674           0 :       ROS_ERROR("[ControlManager]: the failsafe controller (%s) is not within the loaded controllers", _failsafe_controller_name_.c_str());
+    1675           0 :       ros::shutdown();
+    1676             :     }
+    1677             :   }
+    1678             : 
+    1679             :   {
+    1680          82 :     auto idx = idxInVector(_eland_controller_name_, _controller_names_);
+    1681             : 
+    1682          82 :     if (idx) {
+    1683          82 :       _eland_controller_idx_ = idx.value();
+    1684             :     } else {
+    1685           0 :       ROS_ERROR("[ControlManager]: the eland controller (%s) is not within the loaded controllers", _eland_controller_name_.c_str());
+    1686           0 :       ros::shutdown();
+    1687             :     }
+    1688             :   }
+    1689             : 
+    1690             :   {
+    1691          82 :     auto idx = idxInVector(_joystick_controller_name_, _controller_names_);
+    1692             : 
+    1693          82 :     if (idx) {
+    1694          82 :       _joystick_controller_idx_ = idx.value();
+    1695             :     } else {
+    1696           0 :       ROS_ERROR("[ControlManager]: the joystick controller (%s) is not within the loaded controllers", _joystick_controller_name_.c_str());
+    1697           0 :       ros::shutdown();
+    1698             :     }
+    1699             :   }
+    1700             : 
+    1701          82 :   if (_bumper_switch_controller_) {
+    1702             : 
+    1703          82 :     auto idx = idxInVector(_bumper_controller_name_, _controller_names_);
+    1704             : 
+    1705          82 :     if (!idx) {
+    1706           0 :       ROS_ERROR("[ControlManager]: the bumper controller (%s) is not within the loaded controllers", _bumper_controller_name_.c_str());
+    1707           0 :       ros::shutdown();
+    1708             :     }
+    1709             :   }
+    1710             : 
+    1711             :   {
+    1712          82 :     auto idx = idxInVector(_joystick_fallback_controller_name_, _controller_names_);
+    1713             : 
+    1714          82 :     if (idx) {
+    1715          82 :       _joystick_fallback_controller_idx_ = idx.value();
+    1716             :     } else {
+    1717           0 :       ROS_ERROR("[ControlManager]: the joystick fallback controller (%s) is not within the loaded controllers", _joystick_fallback_controller_name_.c_str());
+    1718           0 :       ros::shutdown();
+    1719             :     }
+    1720             :   }
+    1721             : 
+    1722             :   // --------------------------------------------------------------
+    1723             :   // |                  activate the NullTracker                  |
+    1724             :   // --------------------------------------------------------------
+    1725             : 
+    1726          82 :   ROS_INFO("[ControlManager]: activating the null tracker");
+    1727             : 
+    1728          82 :   tracker_list_[_null_tracker_idx_]->activate(last_tracker_cmd_);
+    1729          82 :   active_tracker_idx_ = _null_tracker_idx_;
+    1730             : 
+    1731             :   // --------------------------------------------------------------
+    1732             :   // |    activate the eland controller as the first controller   |
+    1733             :   // --------------------------------------------------------------
+    1734             : 
+    1735          82 :   ROS_INFO("[ControlManager]: activating the the eland controller (%s) as the first controller", _controller_names_[_eland_controller_idx_].c_str());
+    1736             : 
+    1737          82 :   controller_list_[_eland_controller_idx_]->activate(last_control_output_);
+    1738          82 :   active_controller_idx_ = _eland_controller_idx_;
+    1739             : 
+    1740             :   // update the time
+    1741             :   {
+    1742         164 :     std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    1743             : 
+    1744          82 :     controller_tracker_switch_time_ = ros::Time::now();
+    1745             :   }
+    1746             : 
+    1747          82 :   output_enabled_ = false;
+    1748             : 
+    1749             :   // | --------------- set the default constraints -------------- |
+    1750             : 
+    1751          82 :   sanitized_constraints_ = current_constraints_;
+    1752          82 :   setConstraints(current_constraints_);
+    1753             : 
+    1754             :   // | ------------------------ profiler ------------------------ |
+    1755             : 
+    1756          82 :   profiler_ = mrs_lib::Profiler(nh_, "ControlManager", _profiler_enabled_);
+    1757             : 
+    1758             :   // | ----------------------- publishers ----------------------- |
+    1759             : 
+    1760          82 :   control_output_publisher_ = OutputPublisher(nh_);
+    1761             : 
+    1762          82 :   ph_controller_diagnostics_             = mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics>(nh_, "controller_diagnostics_out", 1);
+    1763          82 :   ph_tracker_cmd_                        = mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand>(nh_, "tracker_cmd_out", 1);
+    1764          82 :   ph_mrs_odom_input_                     = mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput>(nh_, "estimator_input_out", 1);
+    1765          82 :   ph_control_reference_odom_             = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "control_reference_out", 1);
+    1766          82 :   ph_diagnostics_                        = mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics>(nh_, "diagnostics_out", 1);
+    1767          82 :   ph_offboard_on_                        = mrs_lib::PublisherHandler<std_msgs::Empty>(nh_, "offboard_on_out", 1);
+    1768          82 :   ph_tilt_error_                         = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "tilt_error_out", 1);
+    1769          82 :   ph_mass_estimate_                      = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "mass_estimate_out", 1, false, 10.0);
+    1770          82 :   ph_mass_nominal_                       = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "mass_nominal_out", 1, true);
+    1771          82 :   ph_throttle_                           = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "throttle_out", 1, false, 10.0);
+    1772          82 :   ph_thrust_                             = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "thrust_out", 1, false, 100.0);
+    1773          82 :   ph_control_error_                      = mrs_lib::PublisherHandler<mrs_msgs::ControlError>(nh_, "control_error_out", 1);
+    1774          82 :   ph_safety_area_markers_                = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "safety_area_markers_out", 1, true, 1.0);
+    1775          82 :   ph_safety_area_coordinates_markers_    = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "safety_area_coordinates_markers_out", 1, true, 1.0);
+    1776          82 :   ph_disturbances_markers_               = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "disturbances_markers_out", 1, false, 10.0);
+    1777          82 :   ph_current_constraints_                = mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints>(nh_, "current_constraints_out", 1);
+    1778          82 :   ph_heading_                            = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "heading_out", 1);
+    1779          82 :   ph_speed_                              = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "speed_out", 1, false, 10.0);
+    1780          82 :   pub_debug_original_trajectory_poses_   = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "trajectory_original/poses_out", 1, true);
+    1781          82 :   pub_debug_original_trajectory_markers_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "trajectory_original/markers_out", 1, true);
+    1782             : 
+    1783             :   // | ------------------ publish nominal mass ------------------ |
+    1784             : 
+    1785             :   {
+    1786          82 :     std_msgs::Float64 nominal_mass;
+    1787             : 
+    1788          82 :     nominal_mass.data = _uav_mass_;
+    1789             : 
+    1790          82 :     ph_mass_nominal_.publish(nominal_mass);
+    1791             :   }
+    1792             : 
+    1793             :   // | ----------------------- subscribers ---------------------- |
+    1794             : 
+    1795         164 :   mrs_lib::SubscribeHandlerOptions shopts;
+    1796          82 :   shopts.nh                 = nh_;
+    1797          82 :   shopts.node_name          = "ControlManager";
+    1798          82 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+    1799          82 :   shopts.threadsafe         = true;
+    1800          82 :   shopts.autostart          = true;
+    1801          82 :   shopts.queue_size         = 10;
+    1802          82 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+    1803             : 
+    1804          82 :   if (_state_input_ == INPUT_UAV_STATE) {
+    1805          82 :     sh_uav_state_ = mrs_lib::SubscribeHandler<mrs_msgs::UavState>(shopts, "uav_state_in", &ControlManager::callbackUavState, this);
+    1806           0 :   } else if (_state_input_ == INPUT_ODOMETRY) {
+    1807           0 :     sh_odometry_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_in", &ControlManager::callbackOdometry, this);
+    1808             :   }
+    1809             : 
+    1810          82 :   if (_odometry_innovation_check_enabled_) {
+    1811          82 :     sh_odometry_innovation_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_innovation_in");
+    1812             :   }
+    1813             : 
+    1814          82 :   sh_bumper_    = mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors>(shopts, "bumper_sectors_in");
+    1815          82 :   sh_max_z_     = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "max_z_in");
+    1816          82 :   sh_joystick_  = mrs_lib::SubscribeHandler<sensor_msgs::Joy>(shopts, "joystick_in", &ControlManager::callbackJoystick, this);
+    1817          82 :   sh_gnss_      = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, "gnss_in", &ControlManager::callbackGNSS, this);
+    1818          82 :   sh_hw_api_rc_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels>(shopts, "hw_api_rc_in", &ControlManager::callbackRC, this);
+    1819             : 
+    1820          82 :   sh_hw_api_status_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts, "hw_api_status_in", &ControlManager::callbackHwApiStatus, this);
+    1821             : 
+    1822             :   // | -------------------- general services -------------------- |
+    1823             : 
+    1824          82 :   service_server_switch_tracker_             = nh_.advertiseService("switch_tracker_in", &ControlManager::callbackSwitchTracker, this);
+    1825          82 :   service_server_switch_controller_          = nh_.advertiseService("switch_controller_in", &ControlManager::callbackSwitchController, this);
+    1826          82 :   service_server_reset_tracker_              = nh_.advertiseService("tracker_reset_static_in", &ControlManager::callbackTrackerResetStatic, this);
+    1827          82 :   service_server_hover_                      = nh_.advertiseService("hover_in", &ControlManager::callbackHover, this);
+    1828          82 :   service_server_ehover_                     = nh_.advertiseService("ehover_in", &ControlManager::callbackEHover, this);
+    1829          82 :   service_server_failsafe_                   = nh_.advertiseService("failsafe_in", &ControlManager::callbackFailsafe, this);
+    1830          82 :   service_server_failsafe_escalating_        = nh_.advertiseService("failsafe_escalating_in", &ControlManager::callbackFailsafeEscalating, this);
+    1831          82 :   service_server_toggle_output_              = nh_.advertiseService("toggle_output_in", &ControlManager::callbackToggleOutput, this);
+    1832          82 :   service_server_arm_                        = nh_.advertiseService("arm_in", &ControlManager::callbackArm, this);
+    1833          82 :   service_server_enable_callbacks_           = nh_.advertiseService("enable_callbacks_in", &ControlManager::callbackEnableCallbacks, this);
+    1834          82 :   service_server_set_constraints_            = nh_.advertiseService("set_constraints_in", &ControlManager::callbackSetConstraints, this);
+    1835          82 :   service_server_use_joystick_               = nh_.advertiseService("use_joystick_in", &ControlManager::callbackUseJoystick, this);
+    1836          82 :   service_server_use_safety_area_            = nh_.advertiseService("use_safety_area_in", &ControlManager::callbackUseSafetyArea, this);
+    1837          82 :   service_server_eland_                      = nh_.advertiseService("eland_in", &ControlManager::callbackEland, this);
+    1838          82 :   service_server_parachute_                  = nh_.advertiseService("parachute_in", &ControlManager::callbackParachute, this);
+    1839          82 :   service_server_set_min_z_                  = nh_.advertiseService("set_min_z_in", &ControlManager::callbackSetMinZ, this);
+    1840          82 :   service_server_transform_reference_        = nh_.advertiseService("transform_reference_in", &ControlManager::callbackTransformReference, this);
+    1841          82 :   service_server_transform_pose_             = nh_.advertiseService("transform_pose_in", &ControlManager::callbackTransformPose, this);
+    1842          82 :   service_server_transform_vector3_          = nh_.advertiseService("transform_vector3_in", &ControlManager::callbackTransformVector3, this);
+    1843          82 :   service_server_bumper_enabler_             = nh_.advertiseService("bumper_in", &ControlManager::callbackEnableBumper, this);
+    1844          82 :   service_server_get_min_z_                  = nh_.advertiseService("get_min_z_in", &ControlManager::callbackGetMinZ, this);
+    1845          82 :   service_server_validate_reference_         = nh_.advertiseService("validate_reference_in", &ControlManager::callbackValidateReference, this);
+    1846          82 :   service_server_validate_reference_2d_      = nh_.advertiseService("validate_reference_2d_in", &ControlManager::callbackValidateReference2d, this);
+    1847          82 :   service_server_validate_reference_list_    = nh_.advertiseService("validate_reference_list_in", &ControlManager::callbackValidateReferenceList, this);
+    1848          82 :   service_server_start_trajectory_tracking_  = nh_.advertiseService("start_trajectory_tracking_in", &ControlManager::callbackStartTrajectoryTracking, this);
+    1849          82 :   service_server_stop_trajectory_tracking_   = nh_.advertiseService("stop_trajectory_tracking_in", &ControlManager::callbackStopTrajectoryTracking, this);
+    1850          82 :   service_server_resume_trajectory_tracking_ = nh_.advertiseService("resume_trajectory_tracking_in", &ControlManager::callbackResumeTrajectoryTracking, this);
+    1851          82 :   service_server_goto_trajectory_start_      = nh_.advertiseService("goto_trajectory_start_in", &ControlManager::callbackGotoTrajectoryStart, this);
+    1852             : 
+    1853          82 :   sch_arming_                 = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "hw_api_arming_out");
+    1854          82 :   sch_eland_                  = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "eland_out");
+    1855          82 :   sch_shutdown_               = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "shutdown_out");
+    1856          82 :   sch_set_odometry_callbacks_ = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "set_odometry_callbacks_out");
+    1857          82 :   sch_ungrip_                 = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "ungrip_out");
+    1858          82 :   sch_parachute_              = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "parachute_out");
+    1859             : 
+    1860             :   // | ---------------- setpoint command services --------------- |
+    1861             : 
+    1862             :   // human callable
+    1863          82 :   service_server_goto_                 = nh_.advertiseService("goto_in", &ControlManager::callbackGoto, this);
+    1864          82 :   service_server_goto_fcu_             = nh_.advertiseService("goto_fcu_in", &ControlManager::callbackGotoFcu, this);
+    1865          82 :   service_server_goto_relative_        = nh_.advertiseService("goto_relative_in", &ControlManager::callbackGotoRelative, this);
+    1866          82 :   service_server_goto_altitude_        = nh_.advertiseService("goto_altitude_in", &ControlManager::callbackGotoAltitude, this);
+    1867          82 :   service_server_set_heading_          = nh_.advertiseService("set_heading_in", &ControlManager::callbackSetHeading, this);
+    1868          82 :   service_server_set_heading_relative_ = nh_.advertiseService("set_heading_relative_in", &ControlManager::callbackSetHeadingRelative, this);
+    1869             : 
+    1870          82 :   service_server_reference_ = nh_.advertiseService("reference_in", &ControlManager::callbackReferenceService, this);
+    1871          82 :   sh_reference_             = mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped>(shopts, "reference_in", &ControlManager::callbackReferenceTopic, this);
+    1872             : 
+    1873          82 :   service_server_velocity_reference_ = nh_.advertiseService("velocity_reference_in", &ControlManager::callbackVelocityReferenceService, this);
+    1874             :   sh_velocity_reference_ =
+    1875          82 :       mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped>(shopts, "velocity_reference_in", &ControlManager::callbackVelocityReferenceTopic, this);
+    1876             : 
+    1877          82 :   service_server_trajectory_reference_ = nh_.advertiseService("trajectory_reference_in", &ControlManager::callbackTrajectoryReferenceService, this);
+    1878             :   sh_trajectory_reference_ =
+    1879          82 :       mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference>(shopts, "trajectory_reference_in", &ControlManager::callbackTrajectoryReferenceTopic, this);
+    1880             : 
+    1881             :   // | --------------------- other services --------------------- |
+    1882             : 
+    1883          82 :   service_server_emergency_reference_ = nh_.advertiseService("emergency_reference_in", &ControlManager::callbackEmergencyReference, this);
+    1884          82 :   service_server_pirouette_           = nh_.advertiseService("pirouette_in", &ControlManager::callbackPirouette, this);
+    1885             : 
+    1886             :   // | ------------------------- timers ------------------------- |
+    1887             : 
+    1888          82 :   timer_status_    = nh_.createTimer(ros::Rate(_status_timer_rate_), &ControlManager::timerStatus, this);
+    1889          82 :   timer_safety_    = nh_.createTimer(ros::Rate(_safety_timer_rate_), &ControlManager::timerSafety, this);
+    1890          82 :   timer_bumper_    = nh_.createTimer(ros::Rate(1.0), &ControlManager::timerBumper, this);
+    1891          82 :   timer_eland_     = nh_.createTimer(ros::Rate(_elanding_timer_rate_), &ControlManager::timerEland, this, false, false);
+    1892          82 :   timer_failsafe_  = nh_.createTimer(ros::Rate(_failsafe_timer_rate_), &ControlManager::timerFailsafe, this, false, false);
+    1893          82 :   timer_pirouette_ = nh_.createTimer(ros::Rate(_pirouette_timer_rate_), &ControlManager::timerPirouette, this, false, false);
+    1894          82 :   timer_joystick_  = nh_.createTimer(ros::Rate(_joystick_timer_rate_), &ControlManager::timerJoystick, this);
+    1895             : 
+    1896             :   // | ----------------------- finish init ---------------------- |
+    1897             : 
+    1898          82 :   if (!param_loader.loadedSuccessfully()) {
+    1899           0 :     ROS_ERROR("[ControlManager]: could not load all parameters!");
+    1900           0 :     ros::shutdown();
+    1901             :   }
+    1902             : 
+    1903          82 :   is_initialized_ = true;
+    1904             : 
+    1905          82 :   ROS_INFO("[ControlManager]: initialized");
+    1906          82 : }
+    1907             : 
+    1908             : //}
+    1909             : 
+    1910             : // --------------------------------------------------------------
+    1911             : // |                           timers                           |
+    1912             : // --------------------------------------------------------------
+    1913             : 
+    1914             : /* timerHwApiCapabilities() //{ */
+    1915             : 
+    1916         143 : void ControlManager::timerHwApiCapabilities(const ros::TimerEvent& event) {
+    1917             : 
+    1918         286 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerHwApiCapabilities", _status_timer_rate_, 1.0, event);
+    1919         286 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerHwApiCapabilities", scope_timer_logger_, scope_timer_enabled_);
+    1920             : 
+    1921         143 :   if (!sh_hw_api_capabilities_.hasMsg()) {
+    1922          61 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: waiting for HW API capabilities");
+    1923          61 :     return;
+    1924             :   }
+    1925             : 
+    1926         164 :   auto hw_ap_capabilities = sh_hw_api_capabilities_.getMsg();
+    1927             : 
+    1928          82 :   ROS_INFO("[ControlManager]: got HW API capabilities, the possible control modes are:");
+    1929             : 
+    1930          82 :   if (hw_ap_capabilities->accepts_actuator_cmd) {
+    1931           3 :     ROS_INFO("[ControlManager]: - actuator command");
+    1932           3 :     _hw_api_inputs_.actuators = true;
+    1933             :   }
+    1934             : 
+    1935          82 :   if (hw_ap_capabilities->accepts_control_group_cmd) {
+    1936           3 :     ROS_INFO("[ControlManager]: - control group command");
+    1937           3 :     _hw_api_inputs_.control_group = true;
+    1938             :   }
+    1939             : 
+    1940          82 :   if (hw_ap_capabilities->accepts_attitude_rate_cmd) {
+    1941          55 :     ROS_INFO("[ControlManager]: - attitude rate command");
+    1942          55 :     _hw_api_inputs_.attitude_rate = true;
+    1943             :   }
+    1944             : 
+    1945          82 :   if (hw_ap_capabilities->accepts_attitude_cmd) {
+    1946          53 :     ROS_INFO("[ControlManager]: - attitude command");
+    1947          53 :     _hw_api_inputs_.attitude = true;
+    1948             :   }
+    1949             : 
+    1950          82 :   if (hw_ap_capabilities->accepts_acceleration_hdg_rate_cmd) {
+    1951           2 :     ROS_INFO("[ControlManager]: - acceleration+hdg rate command");
+    1952           2 :     _hw_api_inputs_.acceleration_hdg_rate = true;
+    1953             :   }
+    1954             : 
+    1955          82 :   if (hw_ap_capabilities->accepts_acceleration_hdg_cmd) {
+    1956           2 :     ROS_INFO("[ControlManager]: - acceleration+hdg command");
+    1957           2 :     _hw_api_inputs_.acceleration_hdg = true;
+    1958             :   }
+    1959             : 
+    1960          82 :   if (hw_ap_capabilities->accepts_velocity_hdg_rate_cmd) {
+    1961           8 :     ROS_INFO("[ControlManager]: - velocityhdg rate command");
+    1962           8 :     _hw_api_inputs_.velocity_hdg_rate = true;
+    1963             :   }
+    1964             : 
+    1965          82 :   if (hw_ap_capabilities->accepts_velocity_hdg_cmd) {
+    1966           4 :     ROS_INFO("[ControlManager]: - velocityhdg command");
+    1967           4 :     _hw_api_inputs_.velocity_hdg = true;
+    1968             :   }
+    1969             : 
+    1970          82 :   if (hw_ap_capabilities->accepts_position_cmd) {
+    1971           2 :     ROS_INFO("[ControlManager]: - position command");
+    1972           2 :     _hw_api_inputs_.position = true;
+    1973             :   }
+    1974             : 
+    1975          82 :   initialize();
+    1976             : 
+    1977          82 :   timer_hw_api_capabilities_.stop();
+    1978             : }
+    1979             : 
+    1980             : //}
+    1981             : 
+    1982             : /* //{ timerStatus() */
+    1983             : 
+    1984       15041 : void ControlManager::timerStatus(const ros::TimerEvent& event) {
+    1985             : 
+    1986       15041 :   if (!is_initialized_)
+    1987           0 :     return;
+    1988             : 
+    1989       45123 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerStatus", _status_timer_rate_, 0.1, event);
+    1990       45123 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerStatus", scope_timer_logger_, scope_timer_enabled_);
+    1991             : 
+    1992             :   // copy member variables
+    1993       30082 :   auto uav_state             = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1994       30082 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    1995       15041 :   auto yaw_error             = mrs_lib::get_mutexed(mutex_attitude_error_, yaw_error_);
+    1996       15041 :   auto position_error        = mrs_lib::get_mutexed(mutex_position_error_, position_error_);
+    1997       15041 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    1998       15041 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    1999             : 
+    2000             :   double uav_x, uav_y, uav_z;
+    2001       15041 :   uav_x = uav_state.pose.position.x;
+    2002       15041 :   uav_y = uav_state.pose.position.y;
+    2003       15041 :   uav_z = uav_state.pose.position.z;
+    2004             : 
+    2005             :   // --------------------------------------------------------------
+    2006             :   // |                      print the status                      |
+    2007             :   // --------------------------------------------------------------
+    2008             : 
+    2009             :   {
+    2010       30082 :     std::string controller = _controller_names_[active_controller_idx];
+    2011       30082 :     std::string tracker    = _tracker_names_[active_tracker_idx];
+    2012       15041 :     double      mass       = last_control_output.diagnostics.total_mass;
+    2013       15041 :     double      bx_b       = last_control_output.diagnostics.disturbance_bx_b;
+    2014       15041 :     double      by_b       = last_control_output.diagnostics.disturbance_by_b;
+    2015       15041 :     double      wx_w       = last_control_output.diagnostics.disturbance_wx_w;
+    2016       15041 :     double      wy_w       = last_control_output.diagnostics.disturbance_wy_w;
+    2017             : 
+    2018       15041 :     ROS_INFO_THROTTLE(5.0, "[ControlManager]: tracker: '%s', controller: '%s', mass: '%.2f kg', disturbances: body [%.2f, %.2f] N, world [%.2f, %.2f] N",
+    2019             :                       tracker.c_str(), controller.c_str(), mass, bx_b, by_b, wx_w, wy_w);
+    2020             :   }
+    2021             : 
+    2022             :   // --------------------------------------------------------------
+    2023             :   // |                   publish the diagnostics                  |
+    2024             :   // --------------------------------------------------------------
+    2025             : 
+    2026       15041 :   publishDiagnostics();
+    2027             : 
+    2028             :   // --------------------------------------------------------------
+    2029             :   // |                publish if the offboard is on               |
+    2030             :   // --------------------------------------------------------------
+    2031             : 
+    2032       15041 :   if (offboard_mode_) {
+    2033             : 
+    2034       10221 :     std_msgs::Empty offboard_on_out;
+    2035             : 
+    2036       10221 :     ph_offboard_on_.publish(offboard_on_out);
+    2037             :   }
+    2038             : 
+    2039             :   // --------------------------------------------------------------
+    2040             :   // |                   publish the tilt error                   |
+    2041             :   // --------------------------------------------------------------
+    2042             :   {
+    2043       30082 :     std::scoped_lock lock(mutex_attitude_error_);
+    2044             : 
+    2045       15041 :     if (tilt_error_) {
+    2046             : 
+    2047       30082 :       mrs_msgs::Float64Stamped tilt_error_out;
+    2048       15041 :       tilt_error_out.header.stamp    = ros::Time::now();
+    2049       15041 :       tilt_error_out.header.frame_id = uav_state.header.frame_id;
+    2050       15041 :       tilt_error_out.value           = (180.0 / M_PI) * tilt_error_.value();
+    2051             : 
+    2052       15041 :       ph_tilt_error_.publish(tilt_error_out);
+    2053             :     }
+    2054             :   }
+    2055             : 
+    2056             :   // --------------------------------------------------------------
+    2057             :   // |                  publish the control error                 |
+    2058             :   // --------------------------------------------------------------
+    2059             : 
+    2060       15041 :   if (position_error) {
+    2061             : 
+    2062        9171 :     Eigen::Vector3d pos_error_value = position_error.value();
+    2063             : 
+    2064       18342 :     mrs_msgs::ControlError msg_out;
+    2065             : 
+    2066        9171 :     msg_out.header.stamp    = ros::Time::now();
+    2067        9171 :     msg_out.header.frame_id = uav_state.header.frame_id;
+    2068             : 
+    2069        9171 :     msg_out.position_errors.x    = pos_error_value[0];
+    2070        9171 :     msg_out.position_errors.y    = pos_error_value[1];
+    2071        9171 :     msg_out.position_errors.z    = pos_error_value[2];
+    2072        9171 :     msg_out.total_position_error = pos_error_value.norm();
+    2073             : 
+    2074        9171 :     if (yaw_error_) {
+    2075        9171 :       msg_out.yaw_error = yaw_error.value();
+    2076             :     }
+    2077             : 
+    2078        9171 :     std::map<std::string, ControllerParams>::iterator it;
+    2079             : 
+    2080        9171 :     it = controllers_.find(_controller_names_[active_controller_idx]);
+    2081             : 
+    2082        9171 :     msg_out.position_eland_threshold    = it->second.eland_threshold;
+    2083        9171 :     msg_out.position_failsafe_threshold = it->second.failsafe_threshold;
+    2084             : 
+    2085        9171 :     ph_control_error_.publish(msg_out);
+    2086             :   }
+    2087             : 
+    2088             :   // --------------------------------------------------------------
+    2089             :   // |                  publish the mass estimate                 |
+    2090             :   // --------------------------------------------------------------
+    2091             : 
+    2092       15041 :   if (last_control_output.diagnostics.mass_estimator) {
+    2093             : 
+    2094        7890 :     std_msgs::Float64 mass_estimate_out;
+    2095        7890 :     mass_estimate_out.data = _uav_mass_ + last_control_output.diagnostics.mass_difference;
+    2096             : 
+    2097        7890 :     ph_mass_estimate_.publish(mass_estimate_out);
+    2098             :   }
+    2099             : 
+    2100             :   // --------------------------------------------------------------
+    2101             :   // |                 publish the current heading                |
+    2102             :   // --------------------------------------------------------------
+    2103             : 
+    2104       15041 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2105             : 
+    2106             :     try {
+    2107             : 
+    2108             :       double heading;
+    2109             : 
+    2110       12902 :       heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    2111             : 
+    2112       25804 :       mrs_msgs::Float64Stamped heading_out;
+    2113       12902 :       heading_out.header = uav_state.header;
+    2114       12902 :       heading_out.value  = heading;
+    2115             : 
+    2116       12902 :       ph_heading_.publish(heading_out);
+    2117             :     }
+    2118           0 :     catch (...) {
+    2119           0 :       ROS_ERROR_THROTTLE(1.0, "exception caught, could not transform heading");
+    2120             :     }
+    2121             :   }
+    2122             : 
+    2123             :   // --------------------------------------------------------------
+    2124             :   // |                  publish the current speed                 |
+    2125             :   // --------------------------------------------------------------
+    2126             : 
+    2127       15041 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2128             : 
+    2129       12902 :     double speed = sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2) + pow(uav_state.velocity.linear.z, 2));
+    2130             : 
+    2131       25804 :     mrs_msgs::Float64Stamped speed_out;
+    2132       12902 :     speed_out.header = uav_state.header;
+    2133       12902 :     speed_out.value  = speed;
+    2134             : 
+    2135       12902 :     ph_speed_.publish(speed_out);
+    2136             :   }
+    2137             : 
+    2138             :   // --------------------------------------------------------------
+    2139             :   // |               publish the safety area markers              |
+    2140             :   // --------------------------------------------------------------
+    2141             : 
+    2142       15041 :   if (use_safety_area_) {
+    2143             : 
+    2144       23976 :     mrs_msgs::ReferenceStamped temp_ref;
+    2145       11988 :     temp_ref.header.frame_id = _safety_area_horizontal_frame_;
+    2146             : 
+    2147       23976 :     geometry_msgs::TransformStamped tf;
+    2148             : 
+    2149       35964 :     auto ret = transformer_->getTransform(_safety_area_horizontal_frame_, "local_origin", ros::Time(0));
+    2150             : 
+    2151       11988 :     if (ret) {
+    2152             : 
+    2153       10093 :       ROS_INFO_ONCE("[ControlManager]: got TFs, publishing safety area markers");
+    2154             : 
+    2155       20186 :       visualization_msgs::MarkerArray safety_area_marker_array;
+    2156       20186 :       visualization_msgs::MarkerArray safety_area_coordinates_marker_array;
+    2157             : 
+    2158       20186 :       mrs_lib::Polygon border = safety_zone_->getBorder();
+    2159             : 
+    2160       20186 :       std::vector<geometry_msgs::Point> border_points_bot_original = border.getPointMessageVector(getMinZ(_safety_area_horizontal_frame_));
+    2161       20186 :       std::vector<geometry_msgs::Point> border_points_top_original = border.getPointMessageVector(getMaxZ(_safety_area_horizontal_frame_));
+    2162             : 
+    2163       20186 :       std::vector<geometry_msgs::Point> border_points_bot_transformed = border_points_bot_original;
+    2164       20186 :       std::vector<geometry_msgs::Point> border_points_top_transformed = border_points_bot_original;
+    2165             : 
+    2166             :       // if we fail in transforming the area at some point
+    2167             :       // do not publish it at all
+    2168       10093 :       bool tf_success = true;
+    2169             : 
+    2170       20186 :       geometry_msgs::TransformStamped tf = ret.value();
+    2171             : 
+    2172             :       /* transform area points to local origin //{ */
+    2173             : 
+    2174             :       // transform border bottom points to local origin
+    2175       50465 :       for (size_t i = 0; i < border_points_bot_original.size(); i++) {
+    2176             : 
+    2177       40372 :         temp_ref.header.frame_id      = _safety_area_horizontal_frame_;
+    2178       40372 :         temp_ref.header.stamp         = ros::Time(0);
+    2179       40372 :         temp_ref.reference.position.x = border_points_bot_original[i].x;
+    2180       40372 :         temp_ref.reference.position.y = border_points_bot_original[i].y;
+    2181       40372 :         temp_ref.reference.position.z = border_points_bot_original[i].z;
+    2182             : 
+    2183       80744 :         if (auto ret = transformer_->transform(temp_ref, tf)) {
+    2184             : 
+    2185       40372 :           temp_ref = ret.value();
+    2186             : 
+    2187       40372 :           border_points_bot_transformed[i].x = temp_ref.reference.position.x;
+    2188       40372 :           border_points_bot_transformed[i].y = temp_ref.reference.position.y;
+    2189       40372 :           border_points_bot_transformed[i].z = temp_ref.reference.position.z;
+    2190             : 
+    2191             :         } else {
+    2192           0 :           tf_success = false;
+    2193             :         }
+    2194             :       }
+    2195             : 
+    2196             :       // transform border top points to local origin
+    2197       50465 :       for (size_t i = 0; i < border_points_top_original.size(); i++) {
+    2198             : 
+    2199       40372 :         temp_ref.header.frame_id      = _safety_area_horizontal_frame_;
+    2200       40372 :         temp_ref.header.stamp         = ros::Time(0);
+    2201       40372 :         temp_ref.reference.position.x = border_points_top_original[i].x;
+    2202       40372 :         temp_ref.reference.position.y = border_points_top_original[i].y;
+    2203       40372 :         temp_ref.reference.position.z = border_points_top_original[i].z;
+    2204             : 
+    2205       80744 :         if (auto ret = transformer_->transform(temp_ref, tf)) {
+    2206             : 
+    2207       40372 :           temp_ref = ret.value();
+    2208             : 
+    2209       40372 :           border_points_top_transformed[i].x = temp_ref.reference.position.x;
+    2210       40372 :           border_points_top_transformed[i].y = temp_ref.reference.position.y;
+    2211       40372 :           border_points_top_transformed[i].z = temp_ref.reference.position.z;
+    2212             : 
+    2213             :         } else {
+    2214           0 :           tf_success = false;
+    2215             :         }
+    2216             :       }
+    2217             : 
+    2218             :       //}
+    2219             : 
+    2220       20186 :       visualization_msgs::Marker safety_area_marker;
+    2221             : 
+    2222       10093 :       safety_area_marker.header.frame_id = _uav_name_ + "/local_origin";
+    2223       10093 :       safety_area_marker.type            = visualization_msgs::Marker::LINE_LIST;
+    2224       10093 :       safety_area_marker.color.a         = 0.15;
+    2225       10093 :       safety_area_marker.scale.x         = 0.2;
+    2226       10093 :       safety_area_marker.color.r         = 1;
+    2227       10093 :       safety_area_marker.color.g         = 0;
+    2228       10093 :       safety_area_marker.color.b         = 0;
+    2229             : 
+    2230       10093 :       safety_area_marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2231             : 
+    2232       20186 :       visualization_msgs::Marker safety_area_coordinates_marker;
+    2233             : 
+    2234       10093 :       safety_area_coordinates_marker.header.frame_id = _uav_name_ + "/local_origin";
+    2235       10093 :       safety_area_coordinates_marker.type            = visualization_msgs::Marker::TEXT_VIEW_FACING;
+    2236       10093 :       safety_area_coordinates_marker.color.a         = 1;
+    2237       10093 :       safety_area_coordinates_marker.scale.z         = 1.0;
+    2238       10093 :       safety_area_coordinates_marker.color.r         = 0;
+    2239       10093 :       safety_area_coordinates_marker.color.g         = 0;
+    2240       10093 :       safety_area_coordinates_marker.color.b         = 0;
+    2241             : 
+    2242       10093 :       safety_area_coordinates_marker.id = 0;
+    2243             : 
+    2244       10093 :       safety_area_coordinates_marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2245             : 
+    2246             :       /* adding safety area points //{ */
+    2247             : 
+    2248             :       // bottom border
+    2249       50465 :       for (size_t i = 0; i < border_points_bot_transformed.size(); i++) {
+    2250             : 
+    2251       40372 :         safety_area_marker.points.push_back(border_points_bot_transformed[i]);
+    2252       40372 :         safety_area_marker.points.push_back(border_points_bot_transformed[(i + 1) % border_points_bot_transformed.size()]);
+    2253             : 
+    2254       80744 :         std::stringstream ss;
+    2255             : 
+    2256       40372 :         if (_safety_area_horizontal_frame_ == "latlon_origin") {
+    2257           0 :           ss << "idx: " << i << std::endl
+    2258           0 :              << std::setprecision(6) << std::fixed << "lat: " << border_points_bot_original[i].x << std::endl
+    2259           0 :              << "lon: " << border_points_bot_original[i].y;
+    2260             :         } else {
+    2261       40372 :           ss << "idx: " << i << std::endl
+    2262       40372 :              << std::setprecision(1) << std::fixed << "x: " << border_points_bot_original[i].x << std::endl
+    2263       40372 :              << "y: " << border_points_bot_original[i].y;
+    2264             :         }
+    2265             : 
+    2266       40372 :         safety_area_coordinates_marker.color.r = 0;
+    2267       40372 :         safety_area_coordinates_marker.color.g = 0;
+    2268       40372 :         safety_area_coordinates_marker.color.b = 0;
+    2269             : 
+    2270       40372 :         safety_area_coordinates_marker.pose.position = border_points_bot_transformed[i];
+    2271       40372 :         safety_area_coordinates_marker.text          = ss.str();
+    2272       40372 :         safety_area_coordinates_marker.id++;
+    2273             : 
+    2274       40372 :         safety_area_coordinates_marker_array.markers.push_back(safety_area_coordinates_marker);
+    2275             :       }
+    2276             : 
+    2277             :       // top border + top/bot edges
+    2278       50465 :       for (size_t i = 0; i < border_points_top_transformed.size(); i++) {
+    2279             : 
+    2280       40372 :         safety_area_marker.points.push_back(border_points_top_transformed[i]);
+    2281       40372 :         safety_area_marker.points.push_back(border_points_top_transformed[(i + 1) % border_points_top_transformed.size()]);
+    2282             : 
+    2283       40372 :         safety_area_marker.points.push_back(border_points_bot_transformed[i]);
+    2284       40372 :         safety_area_marker.points.push_back(border_points_top_transformed[i]);
+    2285             : 
+    2286       80744 :         std::stringstream ss;
+    2287             : 
+    2288       40372 :         if (_safety_area_horizontal_frame_ == "latlon_origin") {
+    2289           0 :           ss << "idx: " << i << std::endl
+    2290           0 :              << std::setprecision(6) << std::fixed << "lat: " << border_points_bot_original[i].x << std::endl
+    2291           0 :              << "lon: " << border_points_bot_original[i].y;
+    2292             :         } else {
+    2293       40372 :           ss << "idx: " << i << std::endl
+    2294       40372 :              << std::setprecision(1) << std::fixed << "x: " << border_points_bot_original[i].x << std::endl
+    2295       40372 :              << "y: " << border_points_bot_original[i].y;
+    2296             :         }
+    2297             : 
+    2298       40372 :         safety_area_coordinates_marker.color.r = 1;
+    2299       40372 :         safety_area_coordinates_marker.color.g = 1;
+    2300       40372 :         safety_area_coordinates_marker.color.b = 1;
+    2301             : 
+    2302       40372 :         safety_area_coordinates_marker.pose.position = border_points_top_transformed[i];
+    2303       40372 :         safety_area_coordinates_marker.text          = ss.str();
+    2304       40372 :         safety_area_coordinates_marker.id++;
+    2305             : 
+    2306       40372 :         safety_area_coordinates_marker_array.markers.push_back(safety_area_coordinates_marker);
+    2307             :       }
+    2308             : 
+    2309             :       //}
+    2310             : 
+    2311       10093 :       if (tf_success) {
+    2312             : 
+    2313       10093 :         safety_area_marker_array.markers.push_back(safety_area_marker);
+    2314             : 
+    2315       10093 :         ph_safety_area_markers_.publish(safety_area_marker_array);
+    2316             : 
+    2317       10093 :         ph_safety_area_coordinates_markers_.publish(safety_area_coordinates_marker_array);
+    2318             :       }
+    2319             : 
+    2320             :     } else {
+    2321        1895 :       ROS_WARN_ONCE("[ControlManager]: missing TFs, can not publish safety area markers");
+    2322             :     }
+    2323             :   }
+    2324             : 
+    2325             :   // --------------------------------------------------------------
+    2326             :   // |              publish the disturbances markers              |
+    2327             :   // --------------------------------------------------------------
+    2328             : 
+    2329       15041 :   if (last_control_output.diagnostics.disturbance_estimator && got_uav_state_) {
+    2330             : 
+    2331       15884 :     visualization_msgs::MarkerArray msg_out;
+    2332             : 
+    2333        7942 :     double id = 0;
+    2334             : 
+    2335        7942 :     double multiplier = 1.0;
+    2336             : 
+    2337        7942 :     Eigen::Quaterniond quat_eigen = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+    2338             : 
+    2339        7942 :     Eigen::Vector3d      vec3d;
+    2340        7942 :     geometry_msgs::Point point;
+    2341             : 
+    2342             :     /* world disturbance //{ */
+    2343             :     {
+    2344             : 
+    2345       15884 :       visualization_msgs::Marker marker;
+    2346             : 
+    2347        7942 :       marker.header.frame_id = uav_state.header.frame_id;
+    2348        7942 :       marker.header.stamp    = ros::Time::now();
+    2349        7942 :       marker.ns              = "control_manager";
+    2350        7942 :       marker.id              = id++;
+    2351        7942 :       marker.type            = visualization_msgs::Marker::ARROW;
+    2352        7942 :       marker.action          = visualization_msgs::Marker::ADD;
+    2353             : 
+    2354             :       /* position //{ */
+    2355             : 
+    2356        7942 :       marker.pose.position.x = 0.0;
+    2357        7942 :       marker.pose.position.y = 0.0;
+    2358        7942 :       marker.pose.position.z = 0.0;
+    2359             : 
+    2360             :       //}
+    2361             : 
+    2362             :       /* orientation //{ */
+    2363             : 
+    2364        7942 :       marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2365             : 
+    2366             :       //}
+    2367             : 
+    2368             :       /* origin //{ */
+    2369        7942 :       point.x = uav_x;
+    2370        7942 :       point.y = uav_y;
+    2371        7942 :       point.z = uav_z;
+    2372             : 
+    2373        7942 :       marker.points.push_back(point);
+    2374             : 
+    2375             :       //}
+    2376             : 
+    2377             :       /* tip //{ */
+    2378             : 
+    2379        7942 :       point.x = uav_x + multiplier * last_control_output.diagnostics.disturbance_wx_w;
+    2380        7942 :       point.y = uav_y + multiplier * last_control_output.diagnostics.disturbance_wy_w;
+    2381        7942 :       point.z = uav_z;
+    2382             : 
+    2383        7942 :       marker.points.push_back(point);
+    2384             : 
+    2385             :       //}
+    2386             : 
+    2387        7942 :       marker.scale.x = 0.05;
+    2388        7942 :       marker.scale.y = 0.05;
+    2389        7942 :       marker.scale.z = 0.05;
+    2390             : 
+    2391        7942 :       marker.color.a = 0.5;
+    2392        7942 :       marker.color.r = 1.0;
+    2393        7942 :       marker.color.g = 0.0;
+    2394        7942 :       marker.color.b = 0.0;
+    2395             : 
+    2396        7942 :       marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    2397             : 
+    2398        7942 :       msg_out.markers.push_back(marker);
+    2399             :     }
+    2400             : 
+    2401             :     //}
+    2402             : 
+    2403             :     /* body disturbance //{ */
+    2404             :     {
+    2405             : 
+    2406       15884 :       visualization_msgs::Marker marker;
+    2407             : 
+    2408        7942 :       marker.header.frame_id = uav_state.header.frame_id;
+    2409        7942 :       marker.header.stamp    = ros::Time::now();
+    2410        7942 :       marker.ns              = "control_manager";
+    2411        7942 :       marker.id              = id++;
+    2412        7942 :       marker.type            = visualization_msgs::Marker::ARROW;
+    2413        7942 :       marker.action          = visualization_msgs::Marker::ADD;
+    2414             : 
+    2415             :       /* position //{ */
+    2416             : 
+    2417        7942 :       marker.pose.position.x = 0.0;
+    2418        7942 :       marker.pose.position.y = 0.0;
+    2419        7942 :       marker.pose.position.z = 0.0;
+    2420             : 
+    2421             :       //}
+    2422             : 
+    2423             :       /* orientation //{ */
+    2424             : 
+    2425        7942 :       marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2426             : 
+    2427             :       //}
+    2428             : 
+    2429             :       /* origin //{ */
+    2430             : 
+    2431        7942 :       point.x = uav_x;
+    2432        7942 :       point.y = uav_y;
+    2433        7942 :       point.z = uav_z;
+    2434             : 
+    2435        7942 :       marker.points.push_back(point);
+    2436             : 
+    2437             :       //}
+    2438             : 
+    2439             :       /* tip //{ */
+    2440             : 
+    2441        7942 :       vec3d << multiplier * last_control_output.diagnostics.disturbance_bx_b, multiplier * last_control_output.diagnostics.disturbance_by_b, 0;
+    2442        7942 :       vec3d = quat_eigen * vec3d;
+    2443             : 
+    2444        7942 :       point.x = uav_x + vec3d[0];
+    2445        7942 :       point.y = uav_y + vec3d[1];
+    2446        7942 :       point.z = uav_z + vec3d[2];
+    2447             : 
+    2448        7942 :       marker.points.push_back(point);
+    2449             : 
+    2450             :       //}
+    2451             : 
+    2452        7942 :       marker.scale.x = 0.05;
+    2453        7942 :       marker.scale.y = 0.05;
+    2454        7942 :       marker.scale.z = 0.05;
+    2455             : 
+    2456        7942 :       marker.color.a = 0.5;
+    2457        7942 :       marker.color.r = 0.0;
+    2458        7942 :       marker.color.g = 1.0;
+    2459        7942 :       marker.color.b = 0.0;
+    2460             : 
+    2461        7942 :       marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+    2462             : 
+    2463        7942 :       msg_out.markers.push_back(marker);
+    2464             :     }
+    2465             : 
+    2466             :     //}
+    2467             : 
+    2468        7942 :     ph_disturbances_markers_.publish(msg_out);
+    2469             :   }
+    2470             : 
+    2471             :   // --------------------------------------------------------------
+    2472             :   // |               publish the current constraints              |
+    2473             :   // --------------------------------------------------------------
+    2474             : 
+    2475       15041 :   if (got_constraints_) {
+    2476             : 
+    2477       12840 :     auto sanitized_constraints = mrs_lib::get_mutexed(mutex_constraints_, sanitized_constraints_);
+    2478             : 
+    2479       12840 :     mrs_msgs::DynamicsConstraints constraints = sanitized_constraints.constraints;
+    2480             : 
+    2481       12840 :     ph_current_constraints_.publish(constraints);
+    2482             :   }
+    2483             : }
+    2484             : 
+    2485             : //}
+    2486             : 
+    2487             : /* //{ timerSafety() */
+    2488             : 
+    2489      276905 : void ControlManager::timerSafety(const ros::TimerEvent& event) {
+    2490             : 
+    2491      276905 :   mrs_lib::AtomicScopeFlag unset_running(running_safety_timer_);
+    2492             : 
+    2493      276907 :   if (!is_initialized_)
+    2494           0 :     return;
+    2495             : 
+    2496      553814 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerSafety", _safety_timer_rate_, 0.05, event);
+    2497      553814 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerSafety", scope_timer_logger_, scope_timer_enabled_);
+    2498             : 
+    2499             :   // copy member variables
+    2500      276907 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2501      276907 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    2502      276907 :   auto [uav_state, uav_yaw]  = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_, uav_yaw_);
+    2503      276907 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    2504      276907 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    2505             : 
+    2506      529287 :   if (!got_uav_state_ || (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) ||
+    2507      252380 :       active_tracker_idx == _null_tracker_idx_) {
+    2508       97634 :     return;
+    2509             :   }
+    2510             : 
+    2511      179273 :   if (odometry_switch_in_progress_) {
+    2512           4 :     ROS_WARN("[ControlManager]: timerSafety tried to run while odometry switch in progress");
+    2513           4 :     return;
+    2514             :   }
+    2515             : 
+    2516             :   // | ------------------------ timeouts ------------------------ |
+    2517             : 
+    2518      179269 :   if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+    2519      179269 :     double missing_for = (ros::Time::now() - sh_uav_state_.lastMsgTime()).toSec();
+    2520             : 
+    2521      179269 :     if (missing_for > _uav_state_max_missing_time_) {
+    2522           7 :       timeoutUavState(missing_for);
+    2523             :     }
+    2524             :   }
+    2525             : 
+    2526      179269 :   if (_state_input_ == INPUT_ODOMETRY && sh_odometry_.hasMsg()) {
+    2527           0 :     double missing_for = (ros::Time::now() - sh_odometry_.lastMsgTime()).toSec();
+    2528             : 
+    2529           0 :     if (missing_for > _uav_state_max_missing_time_) {
+    2530           0 :       timeoutUavState(missing_for);
+    2531             :     }
+    2532             :   }
+    2533             : 
+    2534             :   // | -------------- eland and failsafe thresholds ------------- |
+    2535             : 
+    2536      179269 :   std::map<std::string, ControllerParams>::iterator it;
+    2537      179269 :   it = controllers_.find(_controller_names_[active_controller_idx]);
+    2538             : 
+    2539      179269 :   _eland_threshold_               = it->second.eland_threshold;
+    2540      179269 :   _failsafe_threshold_            = it->second.failsafe_threshold;
+    2541      179269 :   _odometry_innovation_threshold_ = it->second.odometry_innovation_threshold;
+    2542             : 
+    2543             :   // | --------- calculate control errors and tilt angle -------- |
+    2544             : 
+    2545             :   // This means that the timerFailsafe only does its work when Controllers and Trackers produce valid output.
+    2546             :   // Cases when the commands are not valid should be handle in updateControllers() and updateTrackers() methods.
+    2547      179268 :   if (!last_tracker_cmd || !last_control_output.control_output) {
+    2548         153 :     return;
+    2549             :   }
+    2550             : 
+    2551             :   {
+    2552      179115 :     std::scoped_lock lock(mutex_attitude_error_);
+    2553             : 
+    2554      179116 :     tilt_error_ = 0;
+    2555      179116 :     yaw_error_  = 0;
+    2556             :   }
+    2557             : 
+    2558             :   {
+    2559      179116 :     Eigen::Vector3d position_error = Eigen::Vector3d::Zero();
+    2560             : 
+    2561      179116 :     bool position_error_set = false;
+    2562             : 
+    2563      179116 :     if (last_tracker_cmd->use_position_horizontal && !std::holds_alternative<mrs_msgs::HwApiPositionCmd>(last_control_output.control_output.value())) {
+    2564             : 
+    2565      178100 :       position_error[0] = last_tracker_cmd->position.x - uav_state.pose.position.x;
+    2566      178102 :       position_error[1] = last_tracker_cmd->position.y - uav_state.pose.position.y;
+    2567             : 
+    2568      178100 :       position_error_set = true;
+    2569             :     }
+    2570             : 
+    2571      179114 :     if (last_tracker_cmd->use_position_vertical && !std::holds_alternative<mrs_msgs::HwApiPositionCmd>(last_control_output.control_output.value())) {
+    2572             : 
+    2573      178100 :       position_error[2] = last_tracker_cmd->position.z - uav_state.pose.position.z;
+    2574             : 
+    2575      178101 :       position_error_set = true;
+    2576             :     }
+    2577             : 
+    2578      179115 :     if (position_error_set) {
+    2579             : 
+    2580      178101 :       mrs_lib::set_mutexed(mutex_position_error_, {position_error}, position_error_);
+    2581             :     }
+    2582             :   }
+    2583             : 
+    2584             :   // rotate the drone's z axis
+    2585      179116 :   tf2::Transform uav_state_transform = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+    2586      179116 :   tf2::Vector3   uav_z_in_world      = uav_state_transform * tf2::Vector3(0, 0, 1);
+    2587             : 
+    2588             :   // calculate the angle between the drone's z axis and the world's z axis
+    2589      179116 :   double tilt_angle = acos(uav_z_in_world.dot(tf2::Vector3(0, 0, 1)));
+    2590             : 
+    2591             :   // | ------------ calculate the tilt and yaw error ------------ |
+    2592             : 
+    2593             :   // | --------------------- the tilt error --------------------- |
+    2594             : 
+    2595      179116 :   if (last_control_output.desired_orientation) {
+    2596             : 
+    2597             :     // calculate the desired drone's z axis in the world frame
+    2598      154523 :     tf2::Transform attitude_cmd_transform = mrs_lib::AttitudeConverter(last_control_output.desired_orientation.value());
+    2599      154523 :     tf2::Vector3   uav_z_in_world_desired = attitude_cmd_transform * tf2::Vector3(0, 0, 1);
+    2600             : 
+    2601             :     {
+    2602      154523 :       std::scoped_lock lock(mutex_attitude_error_);
+    2603             : 
+    2604             :       // calculate the angle between the drone's z axis and the world's z axis
+    2605      154523 :       tilt_error_ = acos(uav_z_in_world.dot(uav_z_in_world_desired));
+    2606             : 
+    2607             :       // calculate the yaw error
+    2608      154523 :       double cmd_yaw = mrs_lib::AttitudeConverter(last_control_output.desired_orientation.value()).getYaw();
+    2609      154523 :       yaw_error_     = fabs(radians::diff(cmd_yaw, uav_yaw));
+    2610             :     }
+    2611             :   }
+    2612             : 
+    2613      179116 :   auto position_error          = mrs_lib::get_mutexed(mutex_position_error_, position_error_);
+    2614      179116 :   auto [tilt_error, yaw_error] = mrs_lib::get_mutexed(mutex_attitude_error_, tilt_error_, yaw_error_);
+    2615             : 
+    2616             :   // --------------------------------------------------------------
+    2617             :   // |   activate the failsafe controller in case of large error  |
+    2618             :   // --------------------------------------------------------------
+    2619             : 
+    2620      179116 :   if (position_error) {
+    2621             : 
+    2622      178102 :     if (position_error->norm() > _failsafe_threshold_ && !failsafe_triggered_) {
+    2623             : 
+    2624          17 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2625             : 
+    2626          17 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2627             : 
+    2628           1 :         if (!failsafe_triggered_) {
+    2629             : 
+    2630           1 :           ROS_ERROR("[ControlManager]: activating failsafe land: control_error=%.2f/%.2f m (x: %.2f, y: %.2f, z: %.2f)", position_error->norm(),
+    2631             :                     _failsafe_threshold_, position_error.value()[0], position_error.value()[1], position_error.value()[2]);
+    2632             : 
+    2633           1 :           failsafe();
+    2634             :         }
+    2635             :       }
+    2636             :     }
+    2637             :   }
+    2638             : 
+    2639             :   // --------------------------------------------------------------
+    2640             :   // |     activate emergency land in case of large innovation    |
+    2641             :   // --------------------------------------------------------------
+    2642             : 
+    2643      179116 :   if (_odometry_innovation_check_enabled_) {
+    2644             : 
+    2645      179116 :     std::optional<nav_msgs::Odometry::ConstPtr> innovation;
+    2646             : 
+    2647      179116 :     if (sh_odometry_innovation_.hasMsg()) {
+    2648      179116 :       innovation = {sh_odometry_innovation_.getMsg()};
+    2649             :     } else {
+    2650           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: missing estimator innnovation but the innovation check is enabled!");
+    2651             :     }
+    2652             : 
+    2653      179115 :     if (innovation) {
+    2654             : 
+    2655      179116 :       auto [x, y, z] = mrs_lib::getPosition(innovation.value());
+    2656             : 
+    2657      179116 :       double heading = 0;
+    2658             :       try {
+    2659      179116 :         heading = mrs_lib::getHeading(innovation.value());
+    2660             :       }
+    2661           0 :       catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+    2662           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception caught: '%s'", e.what());
+    2663             :       }
+    2664             : 
+    2665      179116 :       double last_innovation = mrs_lib::geometry::dist(vec3_t(x, y, z), vec3_t(0, 0, 0));
+    2666             : 
+    2667      179116 :       if (last_innovation > _odometry_innovation_threshold_ || radians::diff(heading, 0) > M_PI_2) {
+    2668             : 
+    2669           1 :         auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2670             : 
+    2671           1 :         if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2672             : 
+    2673           1 :           if (!failsafe_triggered_ && !eland_triggered_) {
+    2674             : 
+    2675           1 :             ROS_ERROR("[ControlManager]: activating emergency land: odometry innovation too large: %.2f/%.2f (x: %.2f, y: %.2f, z: %.2f, heading: %.2f)",
+    2676             :                       last_innovation, _odometry_innovation_threshold_, x, y, z, heading);
+    2677             : 
+    2678           1 :             eland();
+    2679             :           }
+    2680             :         }
+    2681             :       }
+    2682             :     }
+    2683             :   }
+    2684             : 
+    2685             :   // --------------------------------------------------------------
+    2686             :   // |   activate emergency land in case of medium control error  |
+    2687             :   // --------------------------------------------------------------
+    2688             : 
+    2689             :   // | ------------------- tilt control error ------------------- |
+    2690             : 
+    2691      179116 :   if (_tilt_limit_eland_enabled_ && tilt_angle > _tilt_limit_eland_) {
+    2692             : 
+    2693           0 :     auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2694             : 
+    2695           0 :     if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2696             : 
+    2697           0 :       if (!failsafe_triggered_ && !eland_triggered_) {
+    2698             : 
+    2699           0 :         ROS_ERROR("[ControlManager]: activating emergency land: tilt angle too large (%.2f/%.2f deg)", (180.0 / M_PI) * tilt_angle,
+    2700             :                   (180.0 / M_PI) * _tilt_limit_eland_);
+    2701             : 
+    2702           0 :         eland();
+    2703             :       }
+    2704             :     }
+    2705             :   }
+    2706             : 
+    2707             :   // | ----------------- position control error ----------------- |
+    2708             : 
+    2709      179116 :   if (position_error) {
+    2710             : 
+    2711      178102 :     double error_size = position_error->norm();
+    2712             : 
+    2713      178102 :     if (error_size > _eland_threshold_ / 2.0) {
+    2714             : 
+    2715         799 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2716             : 
+    2717         799 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2718             : 
+    2719         576 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2720             : 
+    2721          91 :           ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: releasing payload due to large position error");
+    2722             : 
+    2723          91 :           ungripSrv();
+    2724             :         }
+    2725             :       }
+    2726             :     }
+    2727             : 
+    2728      178102 :     if (error_size > _eland_threshold_) {
+    2729             : 
+    2730         445 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2731             : 
+    2732         445 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2733             : 
+    2734         261 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2735             : 
+    2736           3 :           ROS_ERROR("[ControlManager]: activating emergency land: position error %.2f/%.2f m (error x: %.2f, y: %.2f, z: %.2f)", error_size, _eland_threshold_,
+    2737             :                     position_error.value()[0], position_error.value()[1], position_error.value()[2]);
+    2738             : 
+    2739           3 :           eland();
+    2740             :         }
+    2741             :       }
+    2742             :     }
+    2743             :   }
+    2744             : 
+    2745             :   // | -------------------- yaw control error ------------------- |
+    2746             :   // do not have to mutex the yaw_error_ here since I am filling it in this function
+    2747             : 
+    2748      179116 :   if (_yaw_error_eland_enabled_ && yaw_error) {
+    2749             : 
+    2750      179116 :     if (yaw_error.value() > (_yaw_error_eland_ / 2.0)) {
+    2751             : 
+    2752           0 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2753             : 
+    2754           0 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2755             : 
+    2756           0 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2757             : 
+    2758           0 :           ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: releasing payload: yaw error %.2f/%.2f deg", (180.0 / M_PI) * yaw_error.value(),
+    2759             :                              (180.0 / M_PI) * _yaw_error_eland_ / 2.0);
+    2760             : 
+    2761           0 :           ungripSrv();
+    2762             :         }
+    2763             :       }
+    2764             :     }
+    2765             : 
+    2766      179116 :     if (yaw_error.value() > _yaw_error_eland_) {
+    2767             : 
+    2768           0 :       auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2769             : 
+    2770           0 :       if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+    2771             : 
+    2772           0 :         if (!failsafe_triggered_ && !eland_triggered_) {
+    2773             : 
+    2774           0 :           ROS_ERROR("[ControlManager]: activating emergency land: yaw error %.2f/%.2f deg", (180.0 / M_PI) * yaw_error.value(),
+    2775             :                     (180.0 / M_PI) * _yaw_error_eland_);
+    2776             : 
+    2777           0 :           eland();
+    2778             :         }
+    2779             :       }
+    2780             :     }
+    2781             :   }
+    2782             : 
+    2783             :   // --------------------------------------------------------------
+    2784             :   // |      disarm the drone when the tilt exceeds the limit      |
+    2785             :   // --------------------------------------------------------------
+    2786      179116 :   if (_tilt_limit_disarm_enabled_ && tilt_angle > _tilt_limit_disarm_) {
+    2787             : 
+    2788           0 :     ROS_ERROR("[ControlManager]: tilt angle too large, disarming: tilt angle=%.2f/%.2f deg", (180.0 / M_PI) * tilt_angle, (180.0 / M_PI) * _tilt_limit_disarm_);
+    2789             : 
+    2790           0 :     arming(false);
+    2791             :   }
+    2792             : 
+    2793             :   // --------------------------------------------------------------
+    2794             :   // |     disarm the drone when tilt error exceeds the limit     |
+    2795             :   // --------------------------------------------------------------
+    2796             : 
+    2797      179116 :   if (_tilt_error_disarm_enabled_ && tilt_error) {
+    2798             : 
+    2799      179116 :     auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+    2800             : 
+    2801             :     // the time from the last controller/tracker switch
+    2802             :     // fyi: we should not
+    2803      179116 :     double time_from_ctrl_tracker_switch = (ros::Time::now() - controller_tracker_switch_time).toSec();
+    2804             : 
+    2805             :     // if the tile error is over the threshold
+    2806             :     // && we are not ramping up during takeoff
+    2807      179116 :     if (fabs(tilt_error.value()) > _tilt_error_disarm_threshold_ && !last_control_output.diagnostics.ramping_up) {
+    2808             : 
+    2809             :       // only account for the error if some time passed from the last tracker/controller switch
+    2810           0 :       if (time_from_ctrl_tracker_switch > 1.0) {
+    2811             : 
+    2812             :         // if the threshold was not exceeded before
+    2813           0 :         if (!tilt_error_disarm_over_thr_) {
+    2814             : 
+    2815           0 :           tilt_error_disarm_over_thr_ = true;
+    2816           0 :           tilt_error_disarm_time_     = ros::Time::now();
+    2817             : 
+    2818           0 :           ROS_WARN("[ControlManager]: tilt error exceeded threshold (%.2f/%.2f deg)", (180.0 / M_PI) * tilt_error.value(),
+    2819             :                    (180.0 / M_PI) * _tilt_error_disarm_threshold_);
+    2820             : 
+    2821             :           // if it was exceeded before, just keep it
+    2822             :         } else {
+    2823             : 
+    2824           0 :           ROS_WARN_THROTTLE(0.1, "[ControlManager]: tilt error (%.2f deg) over threshold for %.2f s", (180.0 / M_PI) * tilt_error.value(),
+    2825             :                             (ros::Time::now() - tilt_error_disarm_time_).toSec());
+    2826             :         }
+    2827             : 
+    2828             :         // if the tile error is bad, but the controller just switched,
+    2829             :         // don't think its bad anymore
+    2830             :       } else {
+    2831             : 
+    2832           0 :         tilt_error_disarm_over_thr_ = false;
+    2833           0 :         tilt_error_disarm_time_     = ros::Time::now();
+    2834             :       }
+    2835             : 
+    2836             :       // if the tilt error is fine
+    2837             :     } else {
+    2838             : 
+    2839             :       // make it fine
+    2840      179116 :       tilt_error_disarm_over_thr_ = false;
+    2841      179116 :       tilt_error_disarm_time_     = ros::Time::now();
+    2842             :     }
+    2843             : 
+    2844             :     // calculate the time over the threshold
+    2845      179116 :     double tot = (ros::Time::now() - tilt_error_disarm_time_).toSec();
+    2846             : 
+    2847             :     // if the tot exceeds the limit (and if we are actually over the threshold)
+    2848      179116 :     if (tilt_error_disarm_over_thr_ && (tot > _tilt_error_disarm_timeout_)) {
+    2849             : 
+    2850           0 :       bool is_flying = offboard_mode_ && active_tracker_idx != _null_tracker_idx_;
+    2851             : 
+    2852             :       // only when flying and not in failsafe
+    2853           0 :       if (is_flying) {
+    2854             : 
+    2855           0 :         ROS_ERROR("[ControlManager]: tilt error too large for %.2f s, disarming", tot);
+    2856             : 
+    2857           0 :         toggleOutput(false);
+    2858           0 :         arming(false);
+    2859             :       }
+    2860             :     }
+    2861             :   }
+    2862             : 
+    2863             :   // | --------- dropping out of OFFBOARD in mid flight --------- |
+    2864             : 
+    2865             :   // if we are not in offboard and the drone is in mid air (NullTracker is not active)
+    2866      179117 :   if (offboard_mode_was_true_ && !offboard_mode_ && active_tracker_idx != _null_tracker_idx_) {
+    2867             : 
+    2868           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: we fell out of OFFBOARD in mid air, disabling output");
+    2869             : 
+    2870           0 :     toggleOutput(false);
+    2871             :   }
+    2872             : }  // namespace control_manager
+    2873             : 
+    2874             : //}
+    2875             : 
+    2876             : /* //{ timerEland() */
+    2877             : 
+    2878         426 : void ControlManager::timerEland(const ros::TimerEvent& event) {
+    2879             : 
+    2880         426 :   if (!is_initialized_)
+    2881         129 :     return;
+    2882             : 
+    2883         852 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerEland", _elanding_timer_rate_, 0.01, event);
+    2884         852 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerEland", scope_timer_logger_, scope_timer_enabled_);
+    2885             : 
+    2886             :   // copy member variables
+    2887         426 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2888             : 
+    2889         426 :   if (!last_control_output.control_output) {
+    2890           0 :     return;
+    2891             :   }
+    2892             : 
+    2893         426 :   auto throttle = extractThrottle(last_control_output);
+    2894             : 
+    2895         426 :   if (!throttle) {
+    2896         129 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: TODO: implement landing detection mechanism for the current control modality");
+    2897         129 :     return;
+    2898             :   }
+    2899             : 
+    2900         297 :   if (current_state_landing_ == IDLE_STATE) {
+    2901             : 
+    2902           0 :     return;
+    2903             : 
+    2904         297 :   } else if (current_state_landing_ == LANDING_STATE) {
+    2905             : 
+    2906             :     // --------------------------------------------------------------
+    2907             :     // |                            TODO                            |
+    2908             :     // This section needs work. The throttle landing detection      |
+    2909             :     // mechanism should be extracted and other mechanisms, such     |
+    2910             :     // as velocity-based detection should be used for high          |
+    2911             :     // modalities                                                   |
+    2912             :     // --------------------------------------------------------------
+    2913             : 
+    2914         297 :     if (!last_control_output.control_output) {
+    2915           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: timerEland: last_control_output has not been initialized, returning");
+    2916           0 :       ROS_WARN_THROTTLE(1.0, "[ControlManager]: tip: the RC eland is probably triggered");
+    2917           0 :       return;
+    2918             :     }
+    2919             : 
+    2920             :     // recalculate the mass based on the throttle
+    2921         297 :     throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g;
+    2922         297 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: landing: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+    2923             : 
+    2924             :     // condition for automatic motor turn off
+    2925         297 :     if (((throttle_mass_estimate_ < _elanding_cutoff_mass_factor_ * landing_uav_mass_) || throttle < 0.01)) {
+    2926          66 :       if (!throttle_under_threshold_) {
+    2927             : 
+    2928           3 :         throttle_mass_estimate_first_time_ = ros::Time::now();
+    2929           3 :         throttle_under_threshold_          = true;
+    2930             :       }
+    2931             : 
+    2932          66 :       ROS_INFO_THROTTLE(0.1, "[ControlManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+    2933             : 
+    2934             :     } else {
+    2935         231 :       throttle_mass_estimate_first_time_ = ros::Time::now();
+    2936         231 :       throttle_under_threshold_          = false;
+    2937             :     }
+    2938             : 
+    2939         297 :     if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _elanding_cutoff_timeout_)) {
+    2940             :       // enable callbacks? ... NO
+    2941             : 
+    2942           3 :       ROS_INFO("[ControlManager]: reached cutoff throttle, disabling output");
+    2943           3 :       toggleOutput(false);
+    2944             : 
+    2945             :       // disarm the drone
+    2946           3 :       if (_eland_disarm_enabled_) {
+    2947             : 
+    2948           3 :         ROS_INFO("[ControlManager]: calling for disarm");
+    2949           3 :         arming(false);
+    2950             :       }
+    2951             : 
+    2952           3 :       changeLandingState(IDLE_STATE);
+    2953             : 
+    2954           3 :       ROS_WARN("[ControlManager]: emergency landing finished");
+    2955             : 
+    2956           3 :       ROS_DEBUG("[ControlManager]: stopping eland timer");
+    2957           3 :       timer_eland_.stop();
+    2958           3 :       ROS_DEBUG("[ControlManager]: eland timer stopped");
+    2959             : 
+    2960             :       // we should NOT set eland_triggered_=true
+    2961             :     }
+    2962             :   }
+    2963             : }
+    2964             : 
+    2965             : //}
+    2966             : 
+    2967             : /* //{ timerFailsafe() */
+    2968             : 
+    2969        9720 : void ControlManager::timerFailsafe(const ros::TimerEvent& event) {
+    2970             : 
+    2971        9720 :   if (!is_initialized_) {
+    2972           0 :     return;
+    2973             :   }
+    2974             : 
+    2975        9720 :   ROS_INFO_ONCE("[ControlManager]: timerFailsafe() spinning");
+    2976             : 
+    2977       19440 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerFailsafe", _failsafe_timer_rate_, 0.01, event);
+    2978       19440 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerFailsafe", scope_timer_logger_, scope_timer_enabled_);
+    2979             : 
+    2980             :   // copy member variables
+    2981        9720 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2982             : 
+    2983        9720 :   updateControllers(uav_state);
+    2984             : 
+    2985        9720 :   publish();
+    2986             : 
+    2987        9720 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    2988             : 
+    2989        9720 :   if (!last_control_output.control_output) {
+    2990           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: timerFailsafe: the control output produced by the failsafe controller is empty!");
+    2991           0 :     return;
+    2992             :   }
+    2993             : 
+    2994        9720 :   auto throttle = extractThrottle(last_control_output);
+    2995             : 
+    2996        9720 :   if (!throttle) {
+    2997           0 :     ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: FailsafeTimer: could not extract throttle out of the last control output");
+    2998           0 :     return;
+    2999             :   }
+    3000             : 
+    3001             :   // --------------------------------------------------------------
+    3002             :   // |                            TODO                            |
+    3003             :   // This section needs work. The throttle landing detection      |
+    3004             :   // mechanism should be extracted and other mechanisms, such     |
+    3005             :   // as velocity-based detection should be used for high          |
+    3006             :   // modalities                                                   |
+    3007             :   // --------------------------------------------------------------
+    3008             : 
+    3009        9720 :   double throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g;
+    3010        9720 :   ROS_INFO_THROTTLE(1.0, "[ControlManager]: failsafe: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+    3011             : 
+    3012             :   // condition for automatic motor turn off
+    3013        9720 :   if (((throttle_mass_estimate_ < _elanding_cutoff_mass_factor_ * landing_uav_mass_))) {
+    3014             : 
+    3015        1414 :     if (!throttle_under_threshold_) {
+    3016             : 
+    3017           7 :       throttle_mass_estimate_first_time_ = ros::Time::now();
+    3018           7 :       throttle_under_threshold_          = true;
+    3019             :     }
+    3020             : 
+    3021        1414 :     ROS_INFO_THROTTLE(0.1, "[ControlManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+    3022             : 
+    3023             :   } else {
+    3024             : 
+    3025        8306 :     throttle_mass_estimate_first_time_ = ros::Time::now();
+    3026        8306 :     throttle_under_threshold_          = false;
+    3027             :   }
+    3028             : 
+    3029             :   // condition for automatic motor turn off
+    3030        9720 :   if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _elanding_cutoff_timeout_)) {
+    3031             : 
+    3032           7 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: detecting zero throttle, disarming");
+    3033             : 
+    3034           7 :     arming(false);
+    3035             :   }
+    3036             : }
+    3037             : 
+    3038             : //}
+    3039             : 
+    3040             : /* //{ timerJoystick() */
+    3041             : 
+    3042       54238 : void ControlManager::timerJoystick(const ros::TimerEvent& event) {
+    3043             : 
+    3044       54238 :   if (!is_initialized_) {
+    3045           0 :     return;
+    3046             :   }
+    3047             : 
+    3048      162714 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerJoystick", _status_timer_rate_, 0.05, event);
+    3049      162714 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerJoystick", scope_timer_logger_, scope_timer_enabled_);
+    3050             : 
+    3051      108476 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    3052             : 
+    3053             :   // if start was pressed and held for > 3.0 s
+    3054       54238 :   if (joystick_start_pressed_ && joystick_start_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_start_press_time_).toSec() > 3.0) {
+    3055             : 
+    3056           0 :     joystick_start_press_time_ = ros::Time(0);
+    3057             : 
+    3058           0 :     ROS_INFO("[ControlManager]: transitioning to joystick control: activating '%s' and '%s'", _joystick_tracker_name_.c_str(),
+    3059             :              _joystick_controller_name_.c_str());
+    3060             : 
+    3061           0 :     joystick_start_pressed_ = false;
+    3062             : 
+    3063           0 :     switchTracker(_joystick_tracker_name_);
+    3064           0 :     switchController(_joystick_controller_name_);
+    3065             :   }
+    3066             : 
+    3067             :   // if RT+LT were pressed and held for > 0.1 s
+    3068       54238 :   if (joystick_failsafe_pressed_ && joystick_failsafe_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_failsafe_press_time_).toSec() > 0.1) {
+    3069             : 
+    3070           0 :     joystick_failsafe_press_time_ = ros::Time(0);
+    3071             : 
+    3072           0 :     ROS_INFO("[ControlManager]: activating failsafe by joystick");
+    3073             : 
+    3074           0 :     joystick_failsafe_pressed_ = false;
+    3075             : 
+    3076           0 :     failsafe();
+    3077             :   }
+    3078             : 
+    3079             :   // if joypads were pressed and held for > 0.1 s
+    3080       54238 :   if (joystick_eland_pressed_ && joystick_eland_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_eland_press_time_).toSec() > 0.1) {
+    3081             : 
+    3082           0 :     joystick_eland_press_time_ = ros::Time(0);
+    3083             : 
+    3084           0 :     ROS_INFO("[ControlManager]: activating eland by joystick");
+    3085             : 
+    3086           0 :     joystick_failsafe_pressed_ = false;
+    3087             : 
+    3088           0 :     eland();
+    3089             :   }
+    3090             : 
+    3091             :   // if back was pressed and held for > 0.1 s
+    3092       54238 :   if (joystick_back_pressed_ && joystick_back_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_back_press_time_).toSec() > 0.1) {
+    3093             : 
+    3094           0 :     joystick_back_press_time_ = ros::Time(0);
+    3095             : 
+    3096             :     // activate/deactivate the joystick goto functionality
+    3097           0 :     joystick_goto_enabled_ = !joystick_goto_enabled_;
+    3098             : 
+    3099           0 :     ROS_INFO("[ControlManager]: joystick control %s", joystick_goto_enabled_ ? "activated" : "deactivated");
+    3100             :   }
+    3101             : 
+    3102             :   // if the GOTO functionality is enabled...
+    3103       54238 :   if (joystick_goto_enabled_ && sh_joystick_.hasMsg()) {
+    3104             : 
+    3105           0 :     auto joystick_data = sh_joystick_.getMsg();
+    3106             : 
+    3107             :     // create the reference
+    3108             : 
+    3109           0 :     mrs_msgs::Vec4::Request request;
+    3110             : 
+    3111           0 :     if (fabs(joystick_data->axes[_channel_pitch_]) >= 0.05 || fabs(joystick_data->axes[_channel_roll_]) >= 0.05 ||
+    3112           0 :         fabs(joystick_data->axes[_channel_heading_]) >= 0.05 || fabs(joystick_data->axes[_channel_throttle_]) >= 0.05) {
+    3113             : 
+    3114           0 :       if (_joystick_mode_ == 0) {
+    3115             : 
+    3116           0 :         request.goal[REF_X]       = _channel_mult_pitch_ * joystick_data->axes[_channel_pitch_] * _joystick_carrot_distance_;
+    3117           0 :         request.goal[REF_Y]       = _channel_mult_roll_ * joystick_data->axes[_channel_roll_] * _joystick_carrot_distance_;
+    3118           0 :         request.goal[REF_Z]       = _channel_mult_throttle_ * joystick_data->axes[_channel_throttle_];
+    3119           0 :         request.goal[REF_HEADING] = _channel_mult_heading_ * joystick_data->axes[_channel_heading_];
+    3120             : 
+    3121           0 :         mrs_msgs::Vec4::Response response;
+    3122             : 
+    3123           0 :         callbackGotoFcu(request, response);
+    3124             : 
+    3125           0 :       } else if (_joystick_mode_ == 1) {
+    3126             : 
+    3127           0 :         mrs_msgs::TrajectoryReference trajectory;
+    3128             : 
+    3129           0 :         double dt = 0.2;
+    3130             : 
+    3131           0 :         trajectory.fly_now         = true;
+    3132           0 :         trajectory.header.frame_id = "fcu_untilted";
+    3133           0 :         trajectory.use_heading     = true;
+    3134           0 :         trajectory.dt              = dt;
+    3135             : 
+    3136           0 :         mrs_msgs::Reference point;
+    3137           0 :         point.position.x = 0;
+    3138           0 :         point.position.y = 0;
+    3139           0 :         point.position.z = 0;
+    3140           0 :         point.heading    = 0;
+    3141             : 
+    3142           0 :         trajectory.points.push_back(point);
+    3143             : 
+    3144           0 :         double speed = 1.0;
+    3145             : 
+    3146           0 :         for (int i = 0; i < 50; i++) {
+    3147             : 
+    3148           0 :           point.position.x += _channel_mult_pitch_ * joystick_data->axes[_channel_pitch_] * (speed * dt);
+    3149           0 :           point.position.y += _channel_mult_roll_ * joystick_data->axes[_channel_roll_] * (speed * dt);
+    3150           0 :           point.position.z += _channel_mult_throttle_ * joystick_data->axes[_channel_throttle_] * (speed * dt);
+    3151           0 :           point.heading = _channel_mult_heading_ * joystick_data->axes[_channel_heading_];
+    3152             : 
+    3153           0 :           trajectory.points.push_back(point);
+    3154             :         }
+    3155             : 
+    3156           0 :         setTrajectoryReference(trajectory);
+    3157             :       }
+    3158             :     }
+    3159             :   }
+    3160             : 
+    3161       54238 :   if (rc_goto_active_ && last_tracker_cmd && sh_hw_api_rc_.hasMsg()) {
+    3162             : 
+    3163             :     // create the reference
+    3164        1098 :     mrs_msgs::VelocityReferenceStampedSrv::Request request;
+    3165             : 
+    3166         549 :     double des_x       = 0;
+    3167         549 :     double des_y       = 0;
+    3168         549 :     double des_z       = 0;
+    3169         549 :     double des_heading = 0;
+    3170             : 
+    3171         549 :     bool nothing_to_do = true;
+    3172             : 
+    3173             :     // copy member variables
+    3174        1098 :     mrs_msgs::HwApiRcChannelsConstPtr rc_channels = sh_hw_api_rc_.getMsg();
+    3175             : 
+    3176         549 :     if (rc_channels->channels.size() < 4) {
+    3177             : 
+    3178           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC control channel numbers are out of range (the # of channels in rc/in topic is %d)",
+    3179             :                          int(rc_channels->channels.size()));
+    3180           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: tip: this could be caused by the RC failsafe not being configured!");
+    3181             : 
+    3182             :     } else {
+    3183             : 
+    3184         549 :       double tmp_x       = RCChannelToRange(rc_channels->channels[_rc_channel_pitch_], _rc_horizontal_speed_, 0.1);
+    3185         549 :       double tmp_y       = -RCChannelToRange(rc_channels->channels[_rc_channel_roll_], _rc_horizontal_speed_, 0.1);
+    3186         549 :       double tmp_z       = RCChannelToRange(rc_channels->channels[_rc_channel_throttle_], _rc_vertical_speed_, 0.3);
+    3187         549 :       double tmp_heading = -RCChannelToRange(rc_channels->channels[_rc_channel_heading_], _rc_heading_rate_, 0.1);
+    3188             : 
+    3189         549 :       if (abs(tmp_x) > 1e-3) {
+    3190          90 :         des_x         = tmp_x;
+    3191          90 :         nothing_to_do = false;
+    3192             :       }
+    3193             : 
+    3194         549 :       if (abs(tmp_y) > 1e-3) {
+    3195         114 :         des_y         = tmp_y;
+    3196         114 :         nothing_to_do = false;
+    3197             :       }
+    3198             : 
+    3199         549 :       if (abs(tmp_z) > 1e-3) {
+    3200         222 :         des_z         = tmp_z;
+    3201         222 :         nothing_to_do = false;
+    3202             :       }
+    3203             : 
+    3204         549 :       if (abs(tmp_heading) > 1e-3) {
+    3205          45 :         des_heading   = tmp_heading;
+    3206          45 :         nothing_to_do = false;
+    3207             :       }
+    3208             :     }
+    3209             : 
+    3210         549 :     if (!nothing_to_do) {
+    3211             : 
+    3212         471 :       request.reference.header.frame_id = "fcu_untilted";
+    3213             : 
+    3214         471 :       request.reference.reference.use_heading_rate = true;
+    3215             : 
+    3216         471 :       request.reference.reference.velocity.x   = des_x;
+    3217         471 :       request.reference.reference.velocity.y   = des_y;
+    3218         471 :       request.reference.reference.velocity.z   = des_z;
+    3219         471 :       request.reference.reference.heading_rate = des_heading;
+    3220             : 
+    3221         942 :       mrs_msgs::VelocityReferenceStampedSrv::Response response;
+    3222             : 
+    3223             :       // disable callbacks of all trackers
+    3224         471 :       std_srvs::SetBoolRequest req_enable_callbacks;
+    3225             : 
+    3226             :       // enable the callbacks for the active tracker
+    3227         471 :       req_enable_callbacks.data = true;
+    3228             :       {
+    3229         471 :         std::scoped_lock lock(mutex_tracker_list_);
+    3230             : 
+    3231         942 :         tracker_list_[active_tracker_idx_]->enableCallbacks(
+    3232         942 :             std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3233             :       }
+    3234             : 
+    3235         471 :       callbacks_enabled_ = true;
+    3236             : 
+    3237         471 :       callbackVelocityReferenceService(request, response);
+    3238             : 
+    3239         471 :       callbacks_enabled_ = false;
+    3240             : 
+    3241         471 :       ROS_INFO_THROTTLE(1.0, "[ControlManager]: goto by RC with speed x=%.2f, y=%.2f, z=%.2f, heading_rate=%.2f", des_x, des_y, des_z, des_heading);
+    3242             : 
+    3243             :       // disable the callbacks back again
+    3244         471 :       req_enable_callbacks.data = false;
+    3245             :       {
+    3246         471 :         std::scoped_lock lock(mutex_tracker_list_);
+    3247             : 
+    3248         942 :         tracker_list_[active_tracker_idx_]->enableCallbacks(
+    3249         942 :             std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3250             :       }
+    3251             :     }
+    3252             :   }
+    3253             : }
+    3254             : 
+    3255             : //}
+    3256             : 
+    3257             : /* //{ timerBumper() */
+    3258             : 
+    3259        2056 : void ControlManager::timerBumper(const ros::TimerEvent& event) {
+    3260             : 
+    3261        2056 :   if (!is_initialized_)
+    3262        1755 :     return;
+    3263             : 
+    3264        4112 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerBumper", _bumper_timer_rate_, 0.05, event);
+    3265        4112 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerBumper", scope_timer_logger_, scope_timer_enabled_);
+    3266             : 
+    3267             :   // | ---------------------- preconditions --------------------- |
+    3268             : 
+    3269        2056 :   if (!sh_bumper_.hasMsg()) {
+    3270        1751 :     return;
+    3271             :   }
+    3272             : 
+    3273         305 :   if (!bumper_enabled_) {
+    3274           0 :     return;
+    3275             :   }
+    3276             : 
+    3277         305 :   if (!isFlyingNormally() && !bumper_repulsing_) {
+    3278           4 :     return;
+    3279             :   }
+    3280             : 
+    3281         301 :   if (!got_uav_state_) {
+    3282           0 :     return;
+    3283             :   }
+    3284             : 
+    3285         301 :   if (sh_bumper_.hasMsg() && (ros::Time::now() - sh_bumper_.lastMsgTime()).toSec() > 1.0) {
+    3286             : 
+    3287           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: obstacle bumper is missing messages although we got them before");
+    3288             : 
+    3289           0 :     return;
+    3290             :   }
+    3291             : 
+    3292         301 :   timer_bumper_.setPeriod(ros::Duration(1.0 / _bumper_timer_rate_));
+    3293             : 
+    3294             :   // | ------------------ call the bumper logic ----------------- |
+    3295             : 
+    3296         301 :   bumperPushFromObstacle();
+    3297             : }
+    3298             : 
+    3299             : //}
+    3300             : 
+    3301             : /* //{ timerPirouette() */
+    3302             : 
+    3303           0 : void ControlManager::timerPirouette(const ros::TimerEvent& event) {
+    3304             : 
+    3305           0 :   if (!is_initialized_)
+    3306           0 :     return;
+    3307             : 
+    3308           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerPirouette", _pirouette_timer_rate_, 0.01, event);
+    3309           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::timerPirouette", scope_timer_logger_, scope_timer_enabled_);
+    3310             : 
+    3311           0 :   pirouette_iterator_++;
+    3312             : 
+    3313           0 :   double pirouette_duration  = (2 * M_PI) / _pirouette_speed_;
+    3314           0 :   double pirouette_n_steps   = pirouette_duration * _pirouette_timer_rate_;
+    3315           0 :   double pirouette_step_size = (2 * M_PI) / pirouette_n_steps;
+    3316             : 
+    3317           0 :   if (rc_escalating_failsafe_triggered_ || failsafe_triggered_ || eland_triggered_ || (pirouette_iterator_ > pirouette_duration * _pirouette_timer_rate_)) {
+    3318             : 
+    3319           0 :     _pirouette_enabled_ = false;
+    3320           0 :     timer_pirouette_.stop();
+    3321             : 
+    3322           0 :     setCallbacks(true);
+    3323             : 
+    3324           0 :     return;
+    3325             :   }
+    3326             : 
+    3327             :   // set the reference
+    3328           0 :   mrs_msgs::ReferenceStamped reference_request;
+    3329             : 
+    3330           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    3331             : 
+    3332           0 :   reference_request.header.frame_id      = "";
+    3333           0 :   reference_request.header.stamp         = ros::Time(0);
+    3334           0 :   reference_request.reference.position.x = last_tracker_cmd->position.x;
+    3335           0 :   reference_request.reference.position.y = last_tracker_cmd->position.y;
+    3336           0 :   reference_request.reference.position.z = last_tracker_cmd->position.z;
+    3337           0 :   reference_request.reference.heading    = pirouette_initial_heading_ + pirouette_iterator_ * pirouette_step_size;
+    3338             : 
+    3339             :   // enable the callbacks for the active tracker
+    3340             :   {
+    3341           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    3342             : 
+    3343           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    3344           0 :     req_enable_callbacks.data = true;
+    3345             : 
+    3346           0 :     tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3347             : 
+    3348           0 :     callbacks_enabled_ = true;
+    3349             :   }
+    3350             : 
+    3351           0 :   setReference(reference_request);
+    3352             : 
+    3353             :   {
+    3354           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    3355             : 
+    3356             :     // disable the callbacks for the active tracker
+    3357           0 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    3358           0 :     req_enable_callbacks.data = false;
+    3359             : 
+    3360           0 :     tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    3361             : 
+    3362           0 :     callbacks_enabled_ = false;
+    3363             :   }
+    3364             : }
+    3365             : 
+    3366             : //}
+    3367             : 
+    3368             : // --------------------------------------------------------------
+    3369             : // |                           asyncs                           |
+    3370             : // --------------------------------------------------------------
+    3371             : 
+    3372             : /* asyncControl() //{ */
+    3373             : 
+    3374      120881 : void ControlManager::asyncControl(void) {
+    3375             : 
+    3376      120881 :   if (!is_initialized_)
+    3377           0 :     return;
+    3378             : 
+    3379      241762 :   mrs_lib::AtomicScopeFlag unset_running(running_async_control_);
+    3380             : 
+    3381      362643 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("asyncControl");
+    3382      362643 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::asyncControl", scope_timer_logger_, scope_timer_enabled_);
+    3383             : 
+    3384             :   // copy member variables
+    3385      241762 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3386      120881 :   auto current_constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    3387             : 
+    3388      120881 :   if (!failsafe_triggered_) {  // when failsafe is triggered, updateControllers() and publish() is called in timerFailsafe()
+    3389             : 
+    3390             :     // run the safety timer
+    3391             :     // in the case of large control errors, the safety mechanisms will be triggered before the controllers and trackers are updated...
+    3392      111599 :     while (running_safety_timer_) {
+    3393             : 
+    3394         879 :       ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3395         879 :       ros::Duration wait(0.001);
+    3396         879 :       wait.sleep();
+    3397             : 
+    3398         879 :       if (!running_safety_timer_) {
+    3399         872 :         ROS_DEBUG("[ControlManager]: safety timer finished");
+    3400         872 :         break;
+    3401             :       }
+    3402             :     }
+    3403             : 
+    3404      111592 :     ros::TimerEvent safety_timer_event;
+    3405      111592 :     timerSafety(safety_timer_event);
+    3406             : 
+    3407      111592 :     updateTrackers();
+    3408             : 
+    3409      111592 :     updateControllers(uav_state);
+    3410             : 
+    3411      111592 :     if (got_constraints_) {
+    3412             : 
+    3413             :       // update the constraints to trackers, if need to
+    3414      111001 :       auto enforce = enforceControllersConstraints(current_constraints);
+    3415             : 
+    3416      111001 :       if (enforce && !constraints_being_enforced_) {
+    3417             : 
+    3418          48 :         setConstraintsToTrackers(enforce.value());
+    3419          48 :         mrs_lib::set_mutexed(mutex_constraints_, enforce.value(), sanitized_constraints_);
+    3420             : 
+    3421          48 :         constraints_being_enforced_ = true;
+    3422             : 
+    3423          48 :         auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    3424             : 
+    3425          48 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: the controller '%s' is enforcing constraints over the ConstraintManager",
+    3426             :                           _controller_names_[active_controller_idx].c_str());
+    3427             : 
+    3428      110953 :       } else if (!enforce && constraints_being_enforced_) {
+    3429             : 
+    3430          43 :         ROS_INFO_THROTTLE(1.0, "[ControlManager]: constraint values returned to original values");
+    3431             : 
+    3432          43 :         constraints_being_enforced_ = false;
+    3433             : 
+    3434          43 :         mrs_lib::set_mutexed(mutex_constraints_, current_constraints, sanitized_constraints_);
+    3435             : 
+    3436          43 :         setConstraintsToTrackers(current_constraints);
+    3437             :       }
+    3438             :     }
+    3439             : 
+    3440      111592 :     publish();
+    3441             :   }
+    3442             : 
+    3443             :   // if odometry switch happened, we finish it here and turn the safety timer back on
+    3444      120881 :   if (odometry_switch_in_progress_) {
+    3445             : 
+    3446           4 :     ROS_DEBUG("[ControlManager]: starting safety timer");
+    3447           4 :     timer_safety_.start();
+    3448           4 :     ROS_DEBUG("[ControlManager]: safety timer started");
+    3449           4 :     odometry_switch_in_progress_ = false;
+    3450             : 
+    3451             :     {
+    3452           8 :       std::scoped_lock lock(mutex_uav_state_);
+    3453             : 
+    3454           4 :       ROS_INFO("[ControlManager]: odometry after switch: x=%.2f, y=%.2f, z=%.2f, heading=%.2f", uav_state.pose.position.x, uav_state.pose.position.y,
+    3455             :                uav_state.pose.position.z, uav_heading_);
+    3456             :     }
+    3457             :   }
+    3458             : }
+    3459             : 
+    3460             : //}
+    3461             : 
+    3462             : // --------------------------------------------------------------
+    3463             : // |                          callbacks                         |
+    3464             : // --------------------------------------------------------------
+    3465             : 
+    3466             : // | --------------------- topic callbacks -------------------- |
+    3467             : 
+    3468             : /* //{ callbackOdometry() */
+    3469             : 
+    3470           0 : void ControlManager::callbackOdometry(const nav_msgs::Odometry::ConstPtr msg) {
+    3471             : 
+    3472           0 :   if (!is_initialized_)
+    3473           0 :     return;
+    3474             : 
+    3475           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackOdometry");
+    3476           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackOdometry", scope_timer_logger_, scope_timer_enabled_);
+    3477             : 
+    3478           0 :   nav_msgs::OdometryConstPtr odom = msg;
+    3479             : 
+    3480             :   // | ------------------ check for time stamp ------------------ |
+    3481             : 
+    3482             :   {
+    3483           0 :     std::scoped_lock lock(mutex_uav_state_);
+    3484             : 
+    3485           0 :     if (uav_state_.header.stamp == odom->header.stamp) {
+    3486           0 :       return;
+    3487             :     }
+    3488             :   }
+    3489             : 
+    3490             :   // | --------------------- check for nans --------------------- |
+    3491             : 
+    3492           0 :   if (!validateOdometry(*odom, "ControlManager", "odometry")) {
+    3493           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: incoming 'odometry' contains invalid values, throwing it away");
+    3494           0 :     return;
+    3495             :   }
+    3496             : 
+    3497             :   // | ---------------------- frame switch ---------------------- |
+    3498             : 
+    3499             :   /* Odometry frame switch //{ */
+    3500             : 
+    3501             :   // | -- prepare an OdometryConstPtr for trackers & controllers -- |
+    3502             : 
+    3503           0 :   mrs_msgs::UavState uav_state_odom;
+    3504             : 
+    3505           0 :   uav_state_odom.header   = odom->header;
+    3506           0 :   uav_state_odom.pose     = odom->pose.pose;
+    3507           0 :   uav_state_odom.velocity = odom->twist.twist;
+    3508             : 
+    3509             :   // | ----- check for change in odometry frame of reference ---- |
+    3510             : 
+    3511           0 :   if (got_uav_state_) {
+    3512             : 
+    3513           0 :     if (odom->header.frame_id != uav_state_.header.frame_id) {
+    3514             : 
+    3515           0 :       ROS_INFO("[ControlManager]: detecting switch of odometry frame");
+    3516             :       {
+    3517           0 :         std::scoped_lock lock(mutex_uav_state_);
+    3518             : 
+    3519           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,
+    3520             :                  uav_state_.pose.position.z, uav_heading_);
+    3521             :       }
+    3522             : 
+    3523           0 :       odometry_switch_in_progress_ = true;
+    3524             : 
+    3525             :       // we have to stop safety timer, otherwise it will interfere
+    3526           0 :       ROS_DEBUG("[ControlManager]: stopping the safety timer");
+    3527           0 :       timer_safety_.stop();
+    3528           0 :       ROS_DEBUG("[ControlManager]: safety timer stopped");
+    3529             : 
+    3530             :       // wait for the safety timer to stop if its running
+    3531           0 :       while (running_safety_timer_) {
+    3532             : 
+    3533           0 :         ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3534           0 :         ros::Duration wait(0.001);
+    3535           0 :         wait.sleep();
+    3536             : 
+    3537           0 :         if (!running_safety_timer_) {
+    3538           0 :           ROS_DEBUG("[ControlManager]: safety timer finished");
+    3539           0 :           break;
+    3540             :         }
+    3541             :       }
+    3542             : 
+    3543             :       // we have to also for the oneshot control timer to finish
+    3544           0 :       while (running_async_control_) {
+    3545             : 
+    3546           0 :         ROS_DEBUG("[ControlManager]: waiting for control timer to finish");
+    3547           0 :         ros::Duration wait(0.001);
+    3548           0 :         wait.sleep();
+    3549             : 
+    3550           0 :         if (!running_async_control_) {
+    3551           0 :           ROS_DEBUG("[ControlManager]: control timer finished");
+    3552           0 :           break;
+    3553             :         }
+    3554             :       }
+    3555             : 
+    3556             :       {
+    3557           0 :         std::scoped_lock lock(mutex_controller_list_, mutex_tracker_list_);
+    3558             : 
+    3559           0 :         tracker_list_[active_tracker_idx_]->switchOdometrySource(uav_state_odom);
+    3560           0 :         controller_list_[active_controller_idx_]->switchOdometrySource(uav_state_odom);
+    3561             :       }
+    3562             :     }
+    3563             :   }
+    3564             : 
+    3565             :   //}
+    3566             : 
+    3567             :   // | ----------- copy the odometry to the uav_state ----------- |
+    3568             : 
+    3569             :   {
+    3570           0 :     std::scoped_lock lock(mutex_uav_state_);
+    3571             : 
+    3572           0 :     previous_uav_state_ = uav_state_;
+    3573             : 
+    3574           0 :     uav_state_ = mrs_msgs::UavState();
+    3575             : 
+    3576           0 :     uav_state_.header           = odom->header;
+    3577           0 :     uav_state_.pose             = odom->pose.pose;
+    3578           0 :     uav_state_.velocity.angular = odom->twist.twist.angular;
+    3579             : 
+    3580             :     // transform the twist into the header's frame
+    3581             :     {
+    3582             :       // the velocity from the odometry
+    3583           0 :       geometry_msgs::Vector3Stamped speed_child_frame;
+    3584           0 :       speed_child_frame.header.frame_id = odom->child_frame_id;
+    3585           0 :       speed_child_frame.header.stamp    = odom->header.stamp;
+    3586           0 :       speed_child_frame.vector.x        = odom->twist.twist.linear.x;
+    3587           0 :       speed_child_frame.vector.y        = odom->twist.twist.linear.y;
+    3588           0 :       speed_child_frame.vector.z        = odom->twist.twist.linear.z;
+    3589             : 
+    3590           0 :       auto res = transformer_->transformSingle(speed_child_frame, odom->header.frame_id);
+    3591             : 
+    3592           0 :       if (res) {
+    3593           0 :         uav_state_.velocity.linear.x = res.value().vector.x;
+    3594           0 :         uav_state_.velocity.linear.y = res.value().vector.y;
+    3595           0 :         uav_state_.velocity.linear.z = res.value().vector.z;
+    3596             :       } else {
+    3597           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not transform the odometry speed from '%s' to '%s'", odom->child_frame_id.c_str(),
+    3598             :                            odom->header.frame_id.c_str());
+    3599           0 :         return;
+    3600             :       }
+    3601             :     }
+    3602             : 
+    3603             :     // calculate the euler angles
+    3604           0 :     std::tie(uav_roll_, uav_pitch_, uav_yaw_) = mrs_lib::AttitudeConverter(odom->pose.pose.orientation);
+    3605             : 
+    3606             :     try {
+    3607           0 :       uav_heading_ = mrs_lib::AttitudeConverter(odom->pose.pose.orientation).getHeading();
+    3608             :     }
+    3609           0 :     catch (...) {
+    3610           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not calculate UAV heading");
+    3611             :     }
+    3612             : 
+    3613           0 :     transformer_->setDefaultFrame(odom->header.frame_id);
+    3614             : 
+    3615           0 :     got_uav_state_ = true;
+    3616             :   }
+    3617             : 
+    3618             :   // run the control loop asynchronously in an OneShotTimer
+    3619             :   // but only if its not already running
+    3620           0 :   if (!running_async_control_) {
+    3621             : 
+    3622           0 :     running_async_control_ = true;
+    3623             : 
+    3624           0 :     async_control_result_ = std::async(std::launch::async, &ControlManager::asyncControl, this);
+    3625             :   }
+    3626             : }
+    3627             : 
+    3628             : //}
+    3629             : 
+    3630             : /* //{ callbackUavState() */
+    3631             : 
+    3632      145682 : void ControlManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) {
+    3633             : 
+    3634      145682 :   if (!is_initialized_)
+    3635       24088 :     return;
+    3636             : 
+    3637      291364 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackUavState");
+    3638      291364 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackUavState", scope_timer_logger_, scope_timer_enabled_);
+    3639             : 
+    3640      145682 :   mrs_msgs::UavStateConstPtr uav_state = msg;
+    3641             : 
+    3642             :   // | ------------------ check for time stamp ------------------ |
+    3643             : 
+    3644             :   {
+    3645      145682 :     std::scoped_lock lock(mutex_uav_state_);
+    3646             : 
+    3647      145682 :     if (uav_state_.header.stamp == uav_state->header.stamp) {
+    3648       24088 :       return;
+    3649             :     }
+    3650             :   }
+    3651             : 
+    3652             :   // | --------------------- check for nans --------------------- |
+    3653             : 
+    3654      121594 :   if (!validateUavState(*uav_state, "ControlManager", "uav_state")) {
+    3655           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: incoming 'uav_state' contains invalid values, throwing it away");
+    3656           0 :     return;
+    3657             :   }
+    3658             : 
+    3659             :   // | -------------------- check for hiccups ------------------- |
+    3660             : 
+    3661             :   /* hickup detection //{ */
+    3662             : 
+    3663      121594 :   double alpha               = 0.99;
+    3664      121594 :   double alpha2              = 0.666;
+    3665      121594 :   double uav_state_count_lim = 1000;
+    3666             : 
+    3667      121594 :   double uav_state_dt = (ros::Time::now() - previous_uav_state_.header.stamp).toSec();
+    3668             : 
+    3669             :   // belive only reasonable numbers
+    3670      121594 :   if (uav_state_dt <= 1.0) {
+    3671             : 
+    3672      121430 :     uav_state_avg_dt_ = alpha * uav_state_avg_dt_ + (1 - alpha) * uav_state_dt;
+    3673             : 
+    3674      121430 :     if (uav_state_count_ < uav_state_count_lim) {
+    3675       64888 :       uav_state_count_++;
+    3676             :     }
+    3677             :   }
+    3678             : 
+    3679      121594 :   if (uav_state_count_ == uav_state_count_lim) {
+    3680             : 
+    3681             :     /* ROS_INFO_STREAM("[ControlManager]: uav_state_dt = " << uav_state_dt); */
+    3682             : 
+    3683       56594 :     if (uav_state_dt < uav_state_avg_dt_ && uav_state_dt > 0.0001) {
+    3684             : 
+    3685       27405 :       uav_state_hiccup_factor_ = alpha2 * uav_state_hiccup_factor_ + (1 - alpha2) * (uav_state_avg_dt_ / uav_state_dt);
+    3686             : 
+    3687       29189 :     } else if (uav_state_avg_dt_ > 0.0001) {
+    3688             : 
+    3689       29189 :       uav_state_hiccup_factor_ = alpha2 * uav_state_hiccup_factor_ + (1 - alpha2) * (uav_state_dt / uav_state_avg_dt_);
+    3690             :     }
+    3691             : 
+    3692       56594 :     if (uav_state_hiccup_factor_ > 3.141592653) {
+    3693             : 
+    3694             :       /* ROS_ERROR_STREAM_THROTTLE(0.1, "[ControlManager]: hiccup factor = " << uav_state_hiccup_factor_); */
+    3695             : 
+    3696           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: ");
+    3697           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | ------------------------- WARNING ------------------------ |");
+    3698           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |                                                            |");
+    3699           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |            UAV_STATE has a large hiccup factor!            |");
+    3700           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |           hint, hint: you are probably rosbagging          |");
+    3701           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |           lot of data or publishing lot of large           |");
+    3702           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |          messages without mutual nodelet managers.         |");
+    3703           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // |                                                            |");
+    3704           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | ------------------------- WARNING ------------------------ |");
+    3705           0 :       ROS_WARN_THROTTLE(2.0, "[ControlManager]: ");
+    3706             :     }
+    3707             :   }
+    3708             : 
+    3709             :   //}
+    3710             : 
+    3711             :   // | ---------------------- frame switch ---------------------- |
+    3712             : 
+    3713             :   /* frame switch //{ */
+    3714             : 
+    3715             :   // | ----- check for change in odometry frame of reference ---- |
+    3716             : 
+    3717      121594 :   if (got_uav_state_) {
+    3718             : 
+    3719      121512 :     if (uav_state->estimator_iteration != uav_state_.estimator_iteration) {
+    3720             : 
+    3721           4 :       ROS_INFO("[ControlManager]: detecting switch of odometry frame");
+    3722             :       {
+    3723           8 :         std::scoped_lock lock(mutex_uav_state_);
+    3724             : 
+    3725           4 :         ROS_INFO("[ControlManager]: odometry before switch: x=%.2f, y=%.2f, z=%.2f, heading=%.2f", uav_state_.pose.position.x, uav_state_.pose.position.y,
+    3726             :                  uav_state_.pose.position.z, uav_heading_);
+    3727             :       }
+    3728             : 
+    3729           4 :       odometry_switch_in_progress_ = true;
+    3730             : 
+    3731             :       // we have to stop safety timer, otherwise it will interfere
+    3732           4 :       ROS_DEBUG("[ControlManager]: stopping the safety timer");
+    3733           4 :       timer_safety_.stop();
+    3734           4 :       ROS_DEBUG("[ControlManager]: safety timer stopped");
+    3735             : 
+    3736             :       // wait for the safety timer to stop if its running
+    3737           4 :       while (running_safety_timer_) {
+    3738             : 
+    3739           0 :         ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+    3740           0 :         ros::Duration wait(0.001);
+    3741           0 :         wait.sleep();
+    3742             : 
+    3743           0 :         if (!running_safety_timer_) {
+    3744           0 :           ROS_DEBUG("[ControlManager]: safety timer finished");
+    3745           0 :           break;
+    3746             :         }
+    3747             :       }
+    3748             : 
+    3749             :       // we have to also for the oneshot control timer to finish
+    3750           4 :       while (running_async_control_) {
+    3751             : 
+    3752           0 :         ROS_DEBUG("[ControlManager]: waiting for control timer to finish");
+    3753           0 :         ros::Duration wait(0.001);
+    3754           0 :         wait.sleep();
+    3755             : 
+    3756           0 :         if (!running_async_control_) {
+    3757           0 :           ROS_DEBUG("[ControlManager]: control timer finished");
+    3758           0 :           break;
+    3759             :         }
+    3760             :       }
+    3761             : 
+    3762             :       {
+    3763           8 :         std::scoped_lock lock(mutex_controller_list_, mutex_tracker_list_);
+    3764             : 
+    3765           4 :         tracker_list_[active_tracker_idx_]->switchOdometrySource(*uav_state);
+    3766           4 :         controller_list_[active_controller_idx_]->switchOdometrySource(*uav_state);
+    3767             :       }
+    3768             :     }
+    3769             :   }
+    3770             : 
+    3771             :   //}
+    3772             : 
+    3773             :   // --------------------------------------------------------------
+    3774             :   // |           copy the UavState message for later use          |
+    3775             :   // --------------------------------------------------------------
+    3776             : 
+    3777             :   {
+    3778      121594 :     std::scoped_lock lock(mutex_uav_state_);
+    3779             : 
+    3780      121594 :     previous_uav_state_ = uav_state_;
+    3781             : 
+    3782      121594 :     uav_state_ = *uav_state;
+    3783             : 
+    3784      121594 :     std::tie(uav_roll_, uav_pitch_, uav_yaw_) = mrs_lib::AttitudeConverter(uav_state_.pose.orientation);
+    3785             : 
+    3786             :     try {
+    3787      121594 :       uav_heading_ = mrs_lib::AttitudeConverter(uav_state_.pose.orientation).getHeading();
+    3788             :     }
+    3789           0 :     catch (...) {
+    3790           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not calculate UAV heading, not updating it");
+    3791             :     }
+    3792             : 
+    3793      121594 :     transformer_->setDefaultFrame(uav_state->header.frame_id);
+    3794             : 
+    3795      121594 :     got_uav_state_ = true;
+    3796             :   }
+    3797             : 
+    3798             :   // run the control loop asynchronously in an OneShotTimer
+    3799             :   // but only if its not already running
+    3800      121594 :   if (!running_async_control_) {
+    3801             : 
+    3802      120881 :     running_async_control_ = true;
+    3803             : 
+    3804      120881 :     async_control_result_ = std::async(std::launch::async, &ControlManager::asyncControl, this);
+    3805             :   }
+    3806             : }
+    3807             : 
+    3808             : //}
+    3809             : 
+    3810             : /* //{ callbackGNSS() */
+    3811             : 
+    3812       84778 : void ControlManager::callbackGNSS(const sensor_msgs::NavSatFix::ConstPtr msg) {
+    3813             : 
+    3814       84778 :   if (!is_initialized_)
+    3815         100 :     return;
+    3816             : 
+    3817      254034 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGNSS");
+    3818      254034 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGNSS", scope_timer_logger_, scope_timer_enabled_);
+    3819             : 
+    3820       84678 :   transformer_->setLatLon(msg->latitude, msg->longitude);
+    3821             : }
+    3822             : 
+    3823             : //}
+    3824             : 
+    3825             : /* callbackJoystick() //{ */
+    3826             : 
+    3827           0 : void ControlManager::callbackJoystick(const sensor_msgs::Joy::ConstPtr msg) {
+    3828             : 
+    3829           0 :   if (!is_initialized_)
+    3830           0 :     return;
+    3831             : 
+    3832           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackJoystick");
+    3833           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackJoystick", scope_timer_logger_, scope_timer_enabled_);
+    3834             : 
+    3835             :   // copy member variables
+    3836           0 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    3837           0 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    3838             : 
+    3839           0 :   sensor_msgs::JoyConstPtr joystick_data = msg;
+    3840             : 
+    3841             :   // TODO check if the array is smaller than the largest idx
+    3842           0 :   if (joystick_data->buttons.size() == 0 || joystick_data->axes.size() == 0) {
+    3843           0 :     return;
+    3844             :   }
+    3845             : 
+    3846             :   // | ---- switching back to fallback tracker and controller --- |
+    3847             : 
+    3848             :   // if any of the A, B, X, Y buttons are pressed when flying with joystick, switch back to fallback controller and tracker
+    3849           0 :   if ((joystick_data->buttons[_channel_A_] == 1 || joystick_data->buttons[_channel_B_] == 1 || joystick_data->buttons[_channel_X_] == 1 ||
+    3850           0 :        joystick_data->buttons[_channel_Y_] == 1) &&
+    3851           0 :       active_tracker_idx == _joystick_tracker_idx_ && active_controller_idx == _joystick_controller_idx_) {
+    3852             : 
+    3853           0 :     ROS_INFO("[ControlManager]: switching from joystick to normal control");
+    3854             : 
+    3855           0 :     switchTracker(_joystick_fallback_tracker_name_);
+    3856           0 :     switchController(_joystick_fallback_controller_name_);
+    3857             : 
+    3858           0 :     joystick_goto_enabled_ = false;
+    3859             :   }
+    3860             : 
+    3861             :   // | ------- joystick control activation ------- |
+    3862             : 
+    3863             :   // if start button was pressed
+    3864           0 :   if (joystick_data->buttons[_channel_start_] == 1) {
+    3865             : 
+    3866           0 :     if (!joystick_start_pressed_) {
+    3867             : 
+    3868           0 :       ROS_INFO("[ControlManager]: joystick start button pressed");
+    3869             : 
+    3870           0 :       joystick_start_pressed_    = true;
+    3871           0 :       joystick_start_press_time_ = ros::Time::now();
+    3872             :     }
+    3873             : 
+    3874           0 :   } else if (joystick_start_pressed_) {
+    3875             : 
+    3876           0 :     ROS_INFO("[ControlManager]: joystick start button released");
+    3877             : 
+    3878           0 :     joystick_start_pressed_    = false;
+    3879           0 :     joystick_start_press_time_ = ros::Time(0);
+    3880             :   }
+    3881             : 
+    3882             :   // | ---------------- Joystick goto activation ---------------- |
+    3883             : 
+    3884             :   // if back button was pressed
+    3885           0 :   if (joystick_data->buttons[_channel_back_] == 1) {
+    3886             : 
+    3887           0 :     if (!joystick_back_pressed_) {
+    3888             : 
+    3889           0 :       ROS_INFO("[ControlManager]: joystick back button pressed");
+    3890             : 
+    3891           0 :       joystick_back_pressed_    = true;
+    3892           0 :       joystick_back_press_time_ = ros::Time::now();
+    3893             :     }
+    3894             : 
+    3895           0 :   } else if (joystick_back_pressed_) {
+    3896             : 
+    3897           0 :     ROS_INFO("[ControlManager]: joystick back button released");
+    3898             : 
+    3899           0 :     joystick_back_pressed_    = false;
+    3900           0 :     joystick_back_press_time_ = ros::Time(0);
+    3901             :   }
+    3902             : 
+    3903             :   // | ------------------------ Failsafes ----------------------- |
+    3904             : 
+    3905             :   // if LT and RT buttons are both pressed down
+    3906           0 :   if (joystick_data->axes[_channel_LT_] < -0.99 && joystick_data->axes[_channel_RT_] < -0.99) {
+    3907             : 
+    3908           0 :     if (!joystick_failsafe_pressed_) {
+    3909             : 
+    3910           0 :       ROS_INFO("[ControlManager]: joystick Failsafe pressed");
+    3911             : 
+    3912           0 :       joystick_failsafe_pressed_    = true;
+    3913           0 :       joystick_failsafe_press_time_ = ros::Time::now();
+    3914             :     }
+    3915             : 
+    3916           0 :   } else if (joystick_failsafe_pressed_) {
+    3917             : 
+    3918           0 :     ROS_INFO("[ControlManager]: joystick Failsafe released");
+    3919             : 
+    3920           0 :     joystick_failsafe_pressed_    = false;
+    3921           0 :     joystick_failsafe_press_time_ = ros::Time(0);
+    3922             :   }
+    3923             : 
+    3924             :   // if left and right joypads are both pressed down
+    3925           0 :   if (joystick_data->buttons[_channel_L_joy_] == 1 && joystick_data->buttons[_channel_R_joy_] == 1) {
+    3926             : 
+    3927           0 :     if (!joystick_eland_pressed_) {
+    3928             : 
+    3929           0 :       ROS_INFO("[ControlManager]: joystick eland pressed");
+    3930             : 
+    3931           0 :       joystick_eland_pressed_    = true;
+    3932           0 :       joystick_eland_press_time_ = ros::Time::now();
+    3933             :     }
+    3934             : 
+    3935           0 :   } else if (joystick_eland_pressed_) {
+    3936             : 
+    3937           0 :     ROS_INFO("[ControlManager]: joystick eland released");
+    3938             : 
+    3939           0 :     joystick_eland_pressed_    = false;
+    3940           0 :     joystick_eland_press_time_ = ros::Time(0);
+    3941             :   }
+    3942             : }
+    3943             : 
+    3944             : //}
+    3945             : 
+    3946             : /* //{ callbackHwApiStatus() */
+    3947             : 
+    3948      360222 : void ControlManager::callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg) {
+    3949             : 
+    3950      360222 :   if (!is_initialized_)
+    3951         324 :     return;
+    3952             : 
+    3953     1079694 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackHwApiStatus");
+    3954     1079694 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackHwApiStatus", scope_timer_logger_, scope_timer_enabled_);
+    3955             : 
+    3956      719796 :   mrs_msgs::HwApiStatusConstPtr state = msg;
+    3957             : 
+    3958             :   // | ------ detect and print the changes in offboard mode ----- |
+    3959      359898 :   if (state->offboard) {
+    3960             : 
+    3961      250837 :     if (!offboard_mode_) {
+    3962          76 :       offboard_mode_          = true;
+    3963          76 :       offboard_mode_was_true_ = true;
+    3964          76 :       ROS_INFO("[ControlManager]: detected: OFFBOARD mode ON");
+    3965             :     }
+    3966             : 
+    3967             :   } else {
+    3968             : 
+    3969      109061 :     if (offboard_mode_) {
+    3970           0 :       offboard_mode_ = false;
+    3971           0 :       ROS_INFO("[ControlManager]: detected: OFFBOARD mode OFF");
+    3972             :     }
+    3973             :   }
+    3974             : 
+    3975             :   // | --------- detect and print the changes in arming --------- |
+    3976      359898 :   if (state->armed == true) {
+    3977             : 
+    3978      262858 :     if (!armed_) {
+    3979          81 :       armed_ = true;
+    3980          81 :       ROS_INFO("[ControlManager]: detected: vehicle ARMED");
+    3981             :     }
+    3982             : 
+    3983             :   } else {
+    3984             : 
+    3985       97040 :     if (armed_) {
+    3986          18 :       armed_ = false;
+    3987          18 :       ROS_INFO("[ControlManager]: detected: vehicle DISARMED");
+    3988             :     }
+    3989             :   }
+    3990             : }
+    3991             : 
+    3992             : //}
+    3993             : 
+    3994             : /* //{ callbackRC() */
+    3995             : 
+    3996       19866 : void ControlManager::callbackRC(const mrs_msgs::HwApiRcChannels::ConstPtr msg) {
+    3997             : 
+    3998       19866 :   if (!is_initialized_)
+    3999           0 :     return;
+    4000             : 
+    4001       59598 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackRC");
+    4002       59598 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackRC", scope_timer_logger_, scope_timer_enabled_);
+    4003             : 
+    4004       39732 :   mrs_msgs::HwApiRcChannelsConstPtr rc = msg;
+    4005             : 
+    4006       19866 :   ROS_INFO_ONCE("[ControlManager]: getting RC channels");
+    4007             : 
+    4008             :   // | ------------------- rc joystic control ------------------- |
+    4009             : 
+    4010             :   // when the switch change its position
+    4011       19866 :   if (_rc_goto_enabled_) {
+    4012             : 
+    4013       19866 :     if (_rc_joystick_channel_ >= int(rc->channels.size())) {
+    4014             : 
+    4015           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC joystick activation channel number (%d) is out of range [0-%d]", _rc_joystick_channel_,
+    4016             :                          int(rc->channels.size()));
+    4017             : 
+    4018             :     } else {
+    4019             : 
+    4020       19866 :       bool channel_low  = rc->channels[_rc_joystick_channel_] < (0.5 - RC_DEADBAND) ? true : false;
+    4021       19866 :       bool channel_high = rc->channels[_rc_joystick_channel_] > (0.5 + RC_DEADBAND) ? true : false;
+    4022             : 
+    4023       19866 :       if (channel_low) {
+    4024       18077 :         rc_joystick_channel_was_low_ = true;
+    4025             :       }
+    4026             : 
+    4027             :       // rc control activation
+    4028       19866 :       if (!rc_goto_active_) {
+    4029             : 
+    4030       18078 :         if (rc_joystick_channel_last_value_ < (0.5 - RC_DEADBAND) && channel_high) {
+    4031             : 
+    4032           2 :           if (isFlyingNormally()) {
+    4033             : 
+    4034           2 :             ROS_INFO_THROTTLE(1.0, "[ControlManager]: activating RC joystick");
+    4035             : 
+    4036           2 :             callbacks_enabled_ = false;
+    4037             : 
+    4038           2 :             std_srvs::SetBoolRequest req_goto_out;
+    4039           2 :             req_goto_out.data = false;
+    4040             : 
+    4041           2 :             std_srvs::SetBoolRequest req_enable_callbacks;
+    4042           2 :             req_enable_callbacks.data = callbacks_enabled_;
+    4043             : 
+    4044             :             {
+    4045           4 :               std::scoped_lock lock(mutex_tracker_list_);
+    4046             : 
+    4047             :               // disable callbacks of all trackers
+    4048          14 :               for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4049          12 :                 tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4050             :               }
+    4051             :             }
+    4052             : 
+    4053           2 :             rc_goto_active_ = true;
+    4054             : 
+    4055             :           } else {
+    4056             : 
+    4057           0 :             ROS_WARN_THROTTLE(1.0, "[ControlManager]: can not activate RC joystick, not flying normally");
+    4058           2 :           }
+    4059             : 
+    4060       18076 :         } else if (channel_high && !rc_joystick_channel_was_low_) {
+    4061             : 
+    4062           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: can not activate RC joystick, the switch is ON from the beginning");
+    4063             :         }
+    4064             :       }
+    4065             : 
+    4066             :       // rc control deactivation
+    4067       19866 :       if (rc_goto_active_ && channel_low) {
+    4068             : 
+    4069           1 :         ROS_INFO("[ControlManager]: deactivating RC joystick");
+    4070             : 
+    4071           1 :         callbacks_enabled_ = true;
+    4072             : 
+    4073           1 :         std_srvs::SetBoolRequest req_goto_out;
+    4074           1 :         req_goto_out.data = true;
+    4075             : 
+    4076           1 :         std_srvs::SetBoolRequest req_enable_callbacks;
+    4077           1 :         req_enable_callbacks.data = callbacks_enabled_;
+    4078             : 
+    4079             :         {
+    4080           2 :           std::scoped_lock lock(mutex_tracker_list_);
+    4081             : 
+    4082             :           // enable callbacks of all trackers
+    4083           7 :           for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4084           6 :             tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4085             :           }
+    4086             :         }
+    4087             : 
+    4088           1 :         rc_goto_active_ = false;
+    4089             :       }
+    4090             : 
+    4091             :       // do not forget to update the last... variable
+    4092             :       // only do that if its out of the deadband
+    4093       19866 :       if (channel_high || channel_low) {
+    4094       19866 :         rc_joystick_channel_last_value_ = rc->channels[_rc_joystick_channel_];
+    4095             :       }
+    4096             :     }
+    4097             :   }
+    4098             : 
+    4099             :   // | ------------------------ rc eland ------------------------ |
+    4100       19866 :   if (_rc_escalating_failsafe_enabled_) {
+    4101             : 
+    4102       19866 :     if (_rc_escalating_failsafe_channel_ >= int(rc->channels.size())) {
+    4103             : 
+    4104           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC eland channel number (%d) is out of range [0-%d]", _rc_escalating_failsafe_channel_,
+    4105             :                          int(rc->channels.size()));
+    4106             : 
+    4107             :     } else {
+    4108             : 
+    4109       19866 :       if (rc->channels[_rc_escalating_failsafe_channel_] >= _rc_escalating_failsafe_threshold_) {
+    4110             : 
+    4111         138 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: triggering escalating failsafe by RC");
+    4112             : 
+    4113         276 :         auto [success, message] = escalatingFailsafe();
+    4114             : 
+    4115         138 :         if (success) {
+    4116           3 :           rc_escalating_failsafe_triggered_ = true;
+    4117             :         }
+    4118             :       }
+    4119             :     }
+    4120             :   }
+    4121             : }
+    4122             : 
+    4123             : //}
+    4124             : 
+    4125             : // | --------------------- topic timeouts --------------------- |
+    4126             : 
+    4127             : /* timeoutUavState() //{ */
+    4128             : 
+    4129           7 : void ControlManager::timeoutUavState(const double& missing_for) {
+    4130             : 
+    4131          14 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    4132             : 
+    4133           7 :   if (output_enabled_ && last_control_output.control_output && !failsafe_triggered_) {
+    4134             : 
+    4135             :     // We need to fire up timerFailsafe, which will regularly trigger the controllers
+    4136             :     // in place of the callbackUavState/callbackOdometry().
+    4137             : 
+    4138           1 :     ROS_ERROR_THROTTLE(0.1, "[ControlManager]: not receiving uav_state/odometry for %.3f s, initiating failsafe land", missing_for);
+    4139             : 
+    4140           1 :     failsafe();
+    4141             :   }
+    4142           7 : }
+    4143             : 
+    4144             : //}
+    4145             : 
+    4146             : // | -------------------- service callbacks ------------------- |
+    4147             : 
+    4148             : /* //{ callbackSwitchTracker() */
+    4149             : 
+    4150         159 : bool ControlManager::callbackSwitchTracker(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    4151             : 
+    4152         159 :   if (!is_initialized_)
+    4153           0 :     return false;
+    4154             : 
+    4155         159 :   if (failsafe_triggered_ || eland_triggered_) {
+    4156             : 
+    4157           0 :     std::stringstream ss;
+    4158           0 :     ss << "can not switch tracker, eland or failsafe active";
+    4159             : 
+    4160           0 :     res.message = ss.str();
+    4161           0 :     res.success = false;
+    4162             : 
+    4163           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4164             : 
+    4165           0 :     return true;
+    4166             :   }
+    4167             : 
+    4168         159 :   auto [success, response] = switchTracker(req.value);
+    4169             : 
+    4170         159 :   res.success = success;
+    4171         159 :   res.message = response;
+    4172             : 
+    4173         159 :   return true;
+    4174             : }
+    4175             : 
+    4176             : //}
+    4177             : 
+    4178             : /* callbackSwitchController() //{ */
+    4179             : 
+    4180         158 : bool ControlManager::callbackSwitchController(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    4181             : 
+    4182         158 :   if (!is_initialized_)
+    4183           0 :     return false;
+    4184             : 
+    4185         158 :   if (failsafe_triggered_ || eland_triggered_) {
+    4186             : 
+    4187           0 :     std::stringstream ss;
+    4188           0 :     ss << "can not switch controller, eland or failsafe active";
+    4189             : 
+    4190           0 :     res.message = ss.str();
+    4191           0 :     res.success = false;
+    4192             : 
+    4193           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4194             : 
+    4195           0 :     return true;
+    4196             :   }
+    4197             : 
+    4198         158 :   auto [success, response] = switchController(req.value);
+    4199             : 
+    4200         158 :   res.success = success;
+    4201         158 :   res.message = response;
+    4202             : 
+    4203         158 :   return true;
+    4204             : }
+    4205             : 
+    4206             : //}
+    4207             : 
+    4208             : /* //{ callbackSwitchTracker() */
+    4209             : 
+    4210           0 : bool ControlManager::callbackTrackerResetStatic([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4211             : 
+    4212           0 :   if (!is_initialized_)
+    4213           0 :     return false;
+    4214             : 
+    4215           0 :   std::stringstream message;
+    4216             : 
+    4217           0 :   if (failsafe_triggered_ || eland_triggered_) {
+    4218             : 
+    4219           0 :     message << "can not reset tracker, eland or failsafe active";
+    4220             : 
+    4221           0 :     res.message = message.str();
+    4222           0 :     res.success = false;
+    4223             : 
+    4224           0 :     ROS_WARN_STREAM("[ControlManager]: " << message.str());
+    4225             : 
+    4226           0 :     return true;
+    4227             :   }
+    4228             : 
+    4229             :   // reactivate the current tracker
+    4230             :   {
+    4231           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    4232             : 
+    4233           0 :     std::string tracker_name = _tracker_names_[active_tracker_idx_];
+    4234             : 
+    4235           0 :     bool succ = tracker_list_[active_tracker_idx_]->resetStatic();
+    4236             : 
+    4237           0 :     if (succ) {
+    4238           0 :       message << "the tracker '" << tracker_name << "' was reset";
+    4239           0 :       ROS_INFO_STREAM("[ControlManager]: " << message.str());
+    4240             :     } else {
+    4241           0 :       message << "the tracker '" << tracker_name << "' reset failed!";
+    4242           0 :       ROS_ERROR_STREAM("[ControlManager]: " << message.str());
+    4243             :     }
+    4244             :   }
+    4245             : 
+    4246           0 :   res.message = message.str();
+    4247           0 :   res.success = true;
+    4248             : 
+    4249           0 :   return true;
+    4250             : }
+    4251             : 
+    4252             : //}
+    4253             : 
+    4254             : /* //{ callbackEHover() */
+    4255             : 
+    4256           0 : bool ControlManager::callbackEHover([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4257             : 
+    4258           0 :   if (!is_initialized_)
+    4259           0 :     return false;
+    4260             : 
+    4261           0 :   if (failsafe_triggered_ || eland_triggered_) {
+    4262             : 
+    4263           0 :     std::stringstream ss;
+    4264           0 :     ss << "can not switch controller, eland or failsafe active";
+    4265             : 
+    4266           0 :     res.message = ss.str();
+    4267           0 :     res.success = false;
+    4268             : 
+    4269           0 :     ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+    4270             : 
+    4271           0 :     return true;
+    4272             :   }
+    4273             : 
+    4274           0 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: ehover trigger by callback");
+    4275             : 
+    4276           0 :   auto [success, message] = ehover();
+    4277             : 
+    4278           0 :   res.success = success;
+    4279           0 :   res.message = message;
+    4280             : 
+    4281           0 :   return true;
+    4282             : }
+    4283             : 
+    4284             : //}
+    4285             : 
+    4286             : /* callbackFailsafe() //{ */
+    4287             : 
+    4288           4 : bool ControlManager::callbackFailsafe([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4289             : 
+    4290           4 :   if (!is_initialized_)
+    4291           0 :     return false;
+    4292             : 
+    4293           4 :   if (failsafe_triggered_) {
+    4294             : 
+    4295           0 :     std::stringstream ss;
+    4296           0 :     ss << "can not activate failsafe, it is already active";
+    4297             : 
+    4298           0 :     res.message = ss.str();
+    4299           0 :     res.success = false;
+    4300             : 
+    4301           0 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4302             : 
+    4303           0 :     return true;
+    4304             :   }
+    4305             : 
+    4306           4 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: failsafe triggered by callback");
+    4307             : 
+    4308           4 :   auto [success, message] = failsafe();
+    4309             : 
+    4310           4 :   res.success = success;
+    4311           4 :   res.message = message;
+    4312             : 
+    4313           4 :   return true;
+    4314             : }
+    4315             : 
+    4316             : //}
+    4317             : 
+    4318             : /* callbackFailsafeEscalating() //{ */
+    4319             : 
+    4320           7 : bool ControlManager::callbackFailsafeEscalating([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4321             : 
+    4322           7 :   if (!is_initialized_)
+    4323           0 :     return false;
+    4324             : 
+    4325           7 :   if (_service_escalating_failsafe_enabled_) {
+    4326             : 
+    4327           7 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: escalating failsafe triggered by callback");
+    4328             : 
+    4329          14 :     auto [success, message] = escalatingFailsafe();
+    4330             : 
+    4331           7 :     res.success = success;
+    4332           7 :     res.message = message;
+    4333             : 
+    4334             :   } else {
+    4335             : 
+    4336           0 :     std::stringstream ss;
+    4337           0 :     ss << "escalating failsafe is disabled";
+    4338             : 
+    4339           0 :     res.success = false;
+    4340           0 :     res.message = ss.str();
+    4341             : 
+    4342           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: %s", ss.str().c_str());
+    4343             :   }
+    4344             : 
+    4345           7 :   return true;
+    4346             : }
+    4347             : 
+    4348             : //}
+    4349             : 
+    4350             : /* //{ callbackELand() */
+    4351             : 
+    4352           1 : bool ControlManager::callbackEland([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4353             : 
+    4354           1 :   if (!is_initialized_)
+    4355           0 :     return false;
+    4356             : 
+    4357           1 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: eland triggered by callback");
+    4358             : 
+    4359           1 :   auto [success, message] = eland();
+    4360             : 
+    4361           1 :   res.success = success;
+    4362           1 :   res.message = message;
+    4363             : 
+    4364           1 :   return true;
+    4365             : }
+    4366             : 
+    4367             : //}
+    4368             : 
+    4369             : /* //{ callbackParachute() */
+    4370             : 
+    4371           0 : bool ControlManager::callbackParachute([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4372             : 
+    4373           0 :   if (!is_initialized_)
+    4374           0 :     return false;
+    4375             : 
+    4376           0 :   if (!_parachute_enabled_) {
+    4377             : 
+    4378           0 :     std::stringstream ss;
+    4379           0 :     ss << "parachute disabled";
+    4380           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4381           0 :     res.message = ss.str();
+    4382           0 :     res.success = false;
+    4383             :   }
+    4384             : 
+    4385           0 :   ROS_WARN_THROTTLE(1.0, "[ControlManager]: parachute triggered by callback");
+    4386             : 
+    4387           0 :   auto [success, message] = deployParachute();
+    4388             : 
+    4389           0 :   res.success = success;
+    4390           0 :   res.message = message;
+    4391             : 
+    4392           0 :   return true;
+    4393             : }
+    4394             : 
+    4395             : //}
+    4396             : 
+    4397             : /* //{ callbackSetMinZ() */
+    4398             : 
+    4399           0 : bool ControlManager::callbackSetMinZ([[maybe_unused]] mrs_msgs::Float64StampedSrv::Request& req, mrs_msgs::Float64StampedSrv::Response& res) {
+    4400             : 
+    4401           0 :   if (!is_initialized_)
+    4402           0 :     return false;
+    4403             : 
+    4404           0 :   if (!use_safety_area_) {
+    4405           0 :     res.success = true;
+    4406           0 :     res.message = "safety area is disabled";
+    4407           0 :     return true;
+    4408             :   }
+    4409             : 
+    4410             :   // | -------- transform min_z to the safety area frame -------- |
+    4411             : 
+    4412           0 :   mrs_msgs::ReferenceStamped point;
+    4413           0 :   point.header               = req.header;
+    4414           0 :   point.reference.position.z = req.value;
+    4415             : 
+    4416           0 :   auto result = transformer_->transformSingle(point, _safety_area_vertical_frame_);
+    4417             : 
+    4418           0 :   if (result) {
+    4419             : 
+    4420           0 :     _safety_area_min_z_ = result.value().reference.position.z;
+    4421             : 
+    4422           0 :     res.success = true;
+    4423           0 :     res.message = "safety area's min z updated";
+    4424             : 
+    4425             :   } else {
+    4426             : 
+    4427           0 :     res.success = false;
+    4428           0 :     res.message = "could not transform the value to safety area's vertical frame";
+    4429             :   }
+    4430             : 
+    4431           0 :   return true;
+    4432             : }
+    4433             : 
+    4434             : //}
+    4435             : 
+    4436             : /* //{ callbackToggleOutput() */
+    4437             : 
+    4438          84 : bool ControlManager::callbackToggleOutput(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4439             : 
+    4440          84 :   if (!is_initialized_)
+    4441           0 :     return false;
+    4442             : 
+    4443          84 :   ROS_INFO("[ControlManager]: toggling output by service");
+    4444             : 
+    4445             :   // copy member variables
+    4446         168 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4447             : 
+    4448         168 :   std::stringstream ss;
+    4449             : 
+    4450          84 :   bool prereq_check = true;
+    4451             : 
+    4452             :   {
+    4453         168 :     mrs_msgs::ReferenceStamped current_coord;
+    4454          84 :     current_coord.header.frame_id      = uav_state.header.frame_id;
+    4455          84 :     current_coord.reference.position.x = uav_state.pose.position.x;
+    4456          84 :     current_coord.reference.position.y = uav_state.pose.position.y;
+    4457             : 
+    4458          84 :     if (!isPointInSafetyArea2d(current_coord)) {
+    4459           1 :       ss << "cannot toggle output, the UAV is outside of the safety area!";
+    4460           1 :       prereq_check = false;
+    4461             :     }
+    4462             :   }
+    4463             : 
+    4464          84 :   if (req.data && (failsafe_triggered_ || eland_triggered_ || rc_escalating_failsafe_triggered_)) {
+    4465           0 :     ss << "cannot toggle output ON, we landed in emergency";
+    4466           0 :     prereq_check = false;
+    4467             :   }
+    4468             : 
+    4469          84 :   if (!sh_hw_api_status_.hasMsg() || (ros::Time::now() - sh_hw_api_status_.lastMsgTime()).toSec() > 1.0) {
+    4470           0 :     ss << "cannot toggle output ON, missing HW API status!";
+    4471           0 :     prereq_check = false;
+    4472             :   }
+    4473             : 
+    4474          84 :   if (!prereq_check) {
+    4475             : 
+    4476           1 :     res.message = ss.str();
+    4477           1 :     res.success = false;
+    4478             : 
+    4479           1 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4480             : 
+    4481           1 :     return false;
+    4482             : 
+    4483             :   } else {
+    4484             : 
+    4485          83 :     toggleOutput(req.data);
+    4486             : 
+    4487          83 :     ss << "Output: " << (output_enabled_ ? "ON" : "OFF");
+    4488          83 :     res.message = ss.str();
+    4489          83 :     res.success = true;
+    4490             : 
+    4491          83 :     ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4492             : 
+    4493          83 :     publishDiagnostics();
+    4494             : 
+    4495          83 :     return true;
+    4496             :   }
+    4497             : }
+    4498             : 
+    4499             : //}
+    4500             : 
+    4501             : /* callbackArm() //{ */
+    4502             : 
+    4503           6 : bool ControlManager::callbackArm(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4504             : 
+    4505           6 :   if (!is_initialized_)
+    4506           0 :     return false;
+    4507             : 
+    4508           6 :   ROS_INFO("[ControlManager]: arming by service");
+    4509             : 
+    4510          12 :   std::stringstream ss;
+    4511             : 
+    4512           6 :   if (failsafe_triggered_ || eland_triggered_) {
+    4513             : 
+    4514           0 :     ss << "can not " << (req.data ? "arm" : "disarm") << ", eland or failsafe active";
+    4515             : 
+    4516           0 :     res.message = ss.str();
+    4517           0 :     res.success = false;
+    4518             : 
+    4519           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4520             : 
+    4521           0 :     return true;
+    4522             :   }
+    4523             : 
+    4524           6 :   if (req.data) {
+    4525             : 
+    4526           0 :     ss << "this service is not allowed to arm the UAV";
+    4527           0 :     res.success = false;
+    4528           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4529             : 
+    4530             :   } else {
+    4531             : 
+    4532          12 :     auto [success, message] = arming(false);
+    4533             : 
+    4534           6 :     if (success) {
+    4535             : 
+    4536           6 :       ss << "disarmed";
+    4537           6 :       res.success = true;
+    4538           6 :       ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4539             : 
+    4540             :     } else {
+    4541             : 
+    4542           0 :       ss << "could not disarm: " << message;
+    4543           0 :       res.success = false;
+    4544           0 :       ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4545             :     }
+    4546             :   }
+    4547             : 
+    4548           6 :   res.message = ss.str();
+    4549             : 
+    4550           6 :   return true;
+    4551             : }
+    4552             : 
+    4553             : //}
+    4554             : 
+    4555             : /* //{ callbackEnableCallbacks() */
+    4556             : 
+    4557          79 : bool ControlManager::callbackEnableCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4558             : 
+    4559          79 :   if (!is_initialized_)
+    4560           0 :     return false;
+    4561             : 
+    4562          79 :   setCallbacks(req.data);
+    4563             : 
+    4564          79 :   std::stringstream ss;
+    4565             : 
+    4566          79 :   ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+    4567             : 
+    4568          79 :   res.message = ss.str();
+    4569          79 :   res.success = true;
+    4570             : 
+    4571          79 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4572             : 
+    4573          79 :   return true;
+    4574             : }
+    4575             : 
+    4576             : //}
+    4577             : 
+    4578             : /* callbackSetConstraints() //{ */
+    4579             : 
+    4580          82 : bool ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrv::Request& req, mrs_msgs::DynamicsConstraintsSrv::Response& res) {
+    4581             : 
+    4582          82 :   if (!is_initialized_) {
+    4583           0 :     res.message = "not initialized";
+    4584           0 :     res.success = false;
+    4585           0 :     return true;
+    4586             :   }
+    4587             : 
+    4588             :   {
+    4589         164 :     std::scoped_lock lock(mutex_constraints_);
+    4590             : 
+    4591          82 :     current_constraints_ = req;
+    4592             : 
+    4593          82 :     auto enforced = enforceControllersConstraints(current_constraints_);
+    4594             : 
+    4595          82 :     if (enforced) {
+    4596           0 :       sanitized_constraints_      = enforced.value();
+    4597           0 :       constraints_being_enforced_ = true;
+    4598             :     } else {
+    4599          82 :       sanitized_constraints_      = req;
+    4600          82 :       constraints_being_enforced_ = false;
+    4601             :     }
+    4602             : 
+    4603          82 :     got_constraints_ = true;
+    4604             : 
+    4605          82 :     setConstraintsToControllers(current_constraints_);
+    4606          82 :     setConstraintsToTrackers(sanitized_constraints_);
+    4607             :   }
+    4608             : 
+    4609          82 :   res.message = "setting constraints";
+    4610          82 :   res.success = true;
+    4611             : 
+    4612          82 :   return true;
+    4613             : }
+    4614             : 
+    4615             : //}
+    4616             : 
+    4617             : /* //{ callbackEmergencyReference() */
+    4618             : 
+    4619          78 : bool ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    4620             : 
+    4621          78 :   if (!is_initialized_)
+    4622           0 :     return false;
+    4623             : 
+    4624         156 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4625             : 
+    4626          78 :   callbacks_enabled_ = false;
+    4627             : 
+    4628          78 :   mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    4629             : 
+    4630         156 :   std::stringstream ss;
+    4631             : 
+    4632             :   // transform the reference to the current frame
+    4633         156 :   mrs_msgs::ReferenceStamped original_reference;
+    4634          78 :   original_reference.header    = req.header;
+    4635          78 :   original_reference.reference = req.reference;
+    4636             : 
+    4637         156 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    4638             : 
+    4639          78 :   if (!ret) {
+    4640             : 
+    4641           0 :     ss << "the emergency reference could not be transformed";
+    4642             : 
+    4643           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4644           0 :     res.message = ss.str();
+    4645           0 :     res.success = false;
+    4646           0 :     return true;
+    4647             :   }
+    4648             : 
+    4649          78 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    4650             : 
+    4651          78 :   std_srvs::SetBoolRequest req_enable_callbacks;
+    4652             : 
+    4653          78 :   mrs_msgs::ReferenceSrvRequest req_goto_out;
+    4654          78 :   req_goto_out.reference = transformed_reference.reference;
+    4655             : 
+    4656             :   {
+    4657         156 :     std::scoped_lock lock(mutex_tracker_list_);
+    4658             : 
+    4659             :     // disable callbacks of all trackers
+    4660          78 :     req_enable_callbacks.data = false;
+    4661         546 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    4662         468 :       tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4663             :     }
+    4664             : 
+    4665             :     // enable the callbacks for the active tracker
+    4666          78 :     req_enable_callbacks.data = true;
+    4667          78 :     tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4668             : 
+    4669             :     // call the setReference()
+    4670         234 :     tracker_response = tracker_list_[active_tracker_idx_]->setReference(
+    4671         234 :         mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(req_goto_out)));
+    4672             : 
+    4673             :     // disable the callbacks back again
+    4674          78 :     req_enable_callbacks.data = false;
+    4675          78 :     tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    4676             : 
+    4677          78 :     if (tracker_response != mrs_msgs::ReferenceSrvResponse::Ptr()) {
+    4678          78 :       res.message = tracker_response->message;
+    4679          78 :       res.success = tracker_response->success;
+    4680             :     } else {
+    4681           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'setReference()' function!";
+    4682           0 :       res.message = ss.str();
+    4683           0 :       res.success = false;
+    4684             :     }
+    4685             :   }
+    4686             : 
+    4687          78 :   return true;
+    4688             : }
+    4689             : 
+    4690             : //}
+    4691             : 
+    4692             : /* callbackPirouette() //{ */
+    4693             : 
+    4694           0 : bool ControlManager::callbackPirouette([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4695             : 
+    4696           0 :   if (!is_initialized_)
+    4697           0 :     return false;
+    4698             : 
+    4699             :   // copy member variables
+    4700           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    4701             : 
+    4702             :   double uav_heading;
+    4703             :   try {
+    4704           0 :     uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    4705             :   }
+    4706           0 :   catch (...) {
+    4707           0 :     std::stringstream ss;
+    4708           0 :     ss << "could not calculate the UAV heading to initialize the pirouette";
+    4709             : 
+    4710           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    4711             : 
+    4712           0 :     res.message = ss.str();
+    4713           0 :     res.success = false;
+    4714             : 
+    4715           0 :     return false;
+    4716             :   }
+    4717             : 
+    4718           0 :   if (_pirouette_enabled_) {
+    4719           0 :     res.success = false;
+    4720           0 :     res.message = "already active";
+    4721           0 :     return true;
+    4722             :   }
+    4723             : 
+    4724           0 :   if (failsafe_triggered_ || eland_triggered_ || rc_escalating_failsafe_triggered_) {
+    4725             : 
+    4726           0 :     std::stringstream ss;
+    4727           0 :     ss << "can not activate the pirouette, eland or failsafe active";
+    4728             : 
+    4729           0 :     res.message = ss.str();
+    4730           0 :     res.success = false;
+    4731             : 
+    4732           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4733             : 
+    4734           0 :     return true;
+    4735             :   }
+    4736             : 
+    4737           0 :   _pirouette_enabled_ = true;
+    4738             : 
+    4739           0 :   setCallbacks(false);
+    4740             : 
+    4741           0 :   pirouette_initial_heading_ = uav_heading;
+    4742           0 :   pirouette_iterator_        = 0;
+    4743           0 :   timer_pirouette_.start();
+    4744             : 
+    4745           0 :   res.success = true;
+    4746           0 :   res.message = "activated";
+    4747             : 
+    4748           0 :   return true;
+    4749             : }
+    4750             : 
+    4751             : //}
+    4752             : 
+    4753             : /* callbackUseJoystick() //{ */
+    4754             : 
+    4755           0 : bool ControlManager::callbackUseJoystick([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4756             : 
+    4757           0 :   if (!is_initialized_) {
+    4758           0 :     return false;
+    4759             :   }
+    4760             : 
+    4761           0 :   std::stringstream ss;
+    4762             : 
+    4763             :   {
+    4764           0 :     auto [success, response] = switchTracker(_joystick_tracker_name_);
+    4765             : 
+    4766           0 :     if (!success) {
+    4767             : 
+    4768           0 :       ss << "switching to '" << _joystick_tracker_name_ << "' was unsuccessfull: '" << response << "'";
+    4769           0 :       ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4770             : 
+    4771           0 :       res.success = false;
+    4772           0 :       res.message = ss.str();
+    4773             : 
+    4774           0 :       return true;
+    4775             :     }
+    4776             :   }
+    4777             : 
+    4778           0 :   auto [success, response] = switchController(_joystick_controller_name_);
+    4779             : 
+    4780           0 :   if (!success) {
+    4781             : 
+    4782           0 :     ss << "switching to '" << _joystick_controller_name_ << "' was unsuccessfull: '" << response << "'";
+    4783           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4784             : 
+    4785           0 :     res.success = false;
+    4786           0 :     res.message = ss.str();
+    4787             : 
+    4788             :     // switch back to hover tracker
+    4789           0 :     switchTracker(_ehover_tracker_name_);
+    4790             : 
+    4791             :     // switch back to safety controller
+    4792           0 :     switchController(_eland_controller_name_);
+    4793             : 
+    4794           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    4795             : 
+    4796           0 :     return true;
+    4797             :   }
+    4798             : 
+    4799           0 :   ss << "switched to joystick control";
+    4800             : 
+    4801           0 :   res.success = true;
+    4802           0 :   res.message = ss.str();
+    4803             : 
+    4804           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4805             : 
+    4806           0 :   return true;
+    4807             : }
+    4808             : 
+    4809             : //}
+    4810             : 
+    4811             : /* //{ callbackHover() */
+    4812             : 
+    4813           0 : bool ControlManager::callbackHover([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4814             : 
+    4815           0 :   if (!is_initialized_)
+    4816           0 :     return false;
+    4817             : 
+    4818           0 :   auto [success, message] = hover();
+    4819             : 
+    4820           0 :   res.success = success;
+    4821           0 :   res.message = message;
+    4822             : 
+    4823           0 :   return true;
+    4824             : }
+    4825             : 
+    4826             : //}
+    4827             : 
+    4828             : /* //{ callbackStartTrajectoryTracking() */
+    4829             : 
+    4830           1 : bool ControlManager::callbackStartTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4831             : 
+    4832           1 :   if (!is_initialized_)
+    4833           0 :     return false;
+    4834             : 
+    4835           1 :   auto [success, message] = startTrajectoryTracking();
+    4836             : 
+    4837           1 :   res.success = success;
+    4838           1 :   res.message = message;
+    4839             : 
+    4840           1 :   return true;
+    4841             : }
+    4842             : 
+    4843             : //}
+    4844             : 
+    4845             : /* //{ callbackStopTrajectoryTracking() */
+    4846             : 
+    4847           0 : bool ControlManager::callbackStopTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4848             : 
+    4849           0 :   if (!is_initialized_)
+    4850           0 :     return false;
+    4851             : 
+    4852           0 :   auto [success, message] = stopTrajectoryTracking();
+    4853             : 
+    4854           0 :   res.success = success;
+    4855           0 :   res.message = message;
+    4856             : 
+    4857           0 :   return true;
+    4858             : }
+    4859             : 
+    4860             : //}
+    4861             : 
+    4862             : /* //{ callbackResumeTrajectoryTracking() */
+    4863             : 
+    4864           0 : bool ControlManager::callbackResumeTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4865             : 
+    4866           0 :   if (!is_initialized_)
+    4867           0 :     return false;
+    4868             : 
+    4869           0 :   auto [success, message] = resumeTrajectoryTracking();
+    4870             : 
+    4871           0 :   res.success = success;
+    4872           0 :   res.message = message;
+    4873             : 
+    4874           0 :   return true;
+    4875             : }
+    4876             : 
+    4877             : //}
+    4878             : 
+    4879             : /* //{ callbackGotoTrajectoryStart() */
+    4880             : 
+    4881           1 : bool ControlManager::callbackGotoTrajectoryStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+    4882             : 
+    4883           1 :   if (!is_initialized_)
+    4884           0 :     return false;
+    4885             : 
+    4886           1 :   auto [success, message] = gotoTrajectoryStart();
+    4887             : 
+    4888           1 :   res.success = success;
+    4889           1 :   res.message = message;
+    4890             : 
+    4891           1 :   return true;
+    4892             : }
+    4893             : 
+    4894             : //}
+    4895             : 
+    4896             : /* //{ callbackTransformReference() */
+    4897             : 
+    4898           0 : bool ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrv::Request& req, mrs_msgs::TransformReferenceSrv::Response& res) {
+    4899             : 
+    4900           0 :   if (!is_initialized_)
+    4901           0 :     return false;
+    4902             : 
+    4903             :   // transform the reference to the current frame
+    4904           0 :   mrs_msgs::ReferenceStamped transformed_reference = req.reference;
+    4905             : 
+    4906           0 :   if (auto ret = transformer_->transformSingle(transformed_reference, req.frame_id)) {
+    4907             : 
+    4908           0 :     res.reference = ret.value();
+    4909           0 :     res.message   = "transformation successful";
+    4910           0 :     res.success   = true;
+    4911           0 :     return true;
+    4912             : 
+    4913             :   } else {
+    4914             : 
+    4915           0 :     res.message = "the reference could not be transformed";
+    4916           0 :     res.success = false;
+    4917           0 :     return true;
+    4918             :   }
+    4919             : 
+    4920             :   return true;
+    4921             : }
+    4922             : 
+    4923             : //}
+    4924             : 
+    4925             : /* //{ callbackTransformPose() */
+    4926             : 
+    4927           0 : bool ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res) {
+    4928             : 
+    4929           0 :   if (!is_initialized_)
+    4930           0 :     return false;
+    4931             : 
+    4932             :   // transform the reference to the current frame
+    4933           0 :   geometry_msgs::PoseStamped transformed_pose = req.pose;
+    4934             : 
+    4935           0 :   if (auto ret = transformer_->transformSingle(transformed_pose, req.frame_id)) {
+    4936             : 
+    4937           0 :     res.pose    = ret.value();
+    4938           0 :     res.message = "transformation successful";
+    4939           0 :     res.success = true;
+    4940           0 :     return true;
+    4941             : 
+    4942             :   } else {
+    4943             : 
+    4944           0 :     res.message = "the pose could not be transformed";
+    4945           0 :     res.success = false;
+    4946           0 :     return true;
+    4947             :   }
+    4948             : 
+    4949             :   return true;
+    4950             : }
+    4951             : 
+    4952             : //}
+    4953             : 
+    4954             : /* //{ callbackTransformVector3() */
+    4955             : 
+    4956           0 : bool ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3Srv::Request& req, mrs_msgs::TransformVector3Srv::Response& res) {
+    4957             : 
+    4958           0 :   if (!is_initialized_)
+    4959           0 :     return false;
+    4960             : 
+    4961             :   // transform the reference to the current frame
+    4962           0 :   geometry_msgs::Vector3Stamped transformed_vector3 = req.vector;
+    4963             : 
+    4964           0 :   if (auto ret = transformer_->transformSingle(transformed_vector3, req.frame_id)) {
+    4965             : 
+    4966           0 :     res.vector  = ret.value();
+    4967           0 :     res.message = "transformation successful";
+    4968           0 :     res.success = true;
+    4969           0 :     return true;
+    4970             : 
+    4971             :   } else {
+    4972             : 
+    4973           0 :     res.message = "the twist could not be transformed";
+    4974           0 :     res.success = false;
+    4975           0 :     return true;
+    4976             :   }
+    4977             : 
+    4978             :   return true;
+    4979             : }
+    4980             : 
+    4981             : //}
+    4982             : 
+    4983             : /* //{ callbackEnableBumper() */
+    4984             : 
+    4985           0 : bool ControlManager::callbackEnableBumper(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    4986             : 
+    4987           0 :   if (!is_initialized_)
+    4988           0 :     return false;
+    4989             : 
+    4990           0 :   bumper_enabled_ = req.data;
+    4991             : 
+    4992           0 :   std::stringstream ss;
+    4993             : 
+    4994           0 :   ss << "bumper " << (bumper_enabled_ ? "enalbed" : "disabled");
+    4995             : 
+    4996           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    4997             : 
+    4998           0 :   res.success = true;
+    4999           0 :   res.message = ss.str();
+    5000             : 
+    5001           0 :   return true;
+    5002             : }
+    5003             : 
+    5004             : //}
+    5005             : 
+    5006             : /* //{ callbackUseSafetyArea() */
+    5007             : 
+    5008           0 : bool ControlManager::callbackUseSafetyArea(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    5009             : 
+    5010           0 :   if (!is_initialized_)
+    5011           0 :     return false;
+    5012             : 
+    5013           0 :   use_safety_area_ = req.data;
+    5014             : 
+    5015           0 :   std::stringstream ss;
+    5016             : 
+    5017           0 :   ss << "safety area " << (use_safety_area_ ? "enabled" : "disabled");
+    5018             : 
+    5019           0 :   ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    5020             : 
+    5021           0 :   res.success = true;
+    5022           0 :   res.message = ss.str();
+    5023             : 
+    5024           0 :   return true;
+    5025             : }
+    5026             : 
+    5027             : //}
+    5028             : 
+    5029             : /* //{ callbackGetMinZ() */
+    5030             : 
+    5031           0 : bool ControlManager::callbackGetMinZ([[maybe_unused]] mrs_msgs::GetFloat64::Request& req, mrs_msgs::GetFloat64::Response& res) {
+    5032             : 
+    5033           0 :   if (!is_initialized_) {
+    5034           0 :     return false;
+    5035             :   }
+    5036             : 
+    5037           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5038             : 
+    5039           0 :   res.success = true;
+    5040           0 :   res.value   = getMinZ(uav_state.header.frame_id);
+    5041             : 
+    5042           0 :   return true;
+    5043             : }
+    5044             : 
+    5045             : //}
+    5046             : 
+    5047             : /* //{ callbackValidateReference() */
+    5048             : 
+    5049           0 : bool ControlManager::callbackValidateReference(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res) {
+    5050             : 
+    5051           0 :   if (!is_initialized_) {
+    5052           0 :     res.message = "not initialized";
+    5053           0 :     res.success = false;
+    5054           0 :     return true;
+    5055             :   }
+    5056             : 
+    5057           0 :   if (!validateReference(req.reference.reference, "ControlManager", "reference_for_validation")) {
+    5058           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: NaN detected in variable 'req.reference'!!!");
+    5059           0 :     res.message = "NaNs/infs in input!";
+    5060           0 :     res.success = false;
+    5061           0 :     return true;
+    5062             :   }
+    5063             : 
+    5064             :   // copy member variables
+    5065           0 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5066           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5067             : 
+    5068             :   // transform the reference to the current frame
+    5069           0 :   mrs_msgs::ReferenceStamped original_reference;
+    5070           0 :   original_reference.header    = req.reference.header;
+    5071           0 :   original_reference.reference = req.reference.reference;
+    5072             : 
+    5073           0 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5074             : 
+    5075           0 :   if (!ret) {
+    5076             : 
+    5077           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the reference could not be transformed");
+    5078           0 :     res.message = "the reference could not be transformed";
+    5079           0 :     res.success = false;
+    5080           0 :     return true;
+    5081             :   }
+    5082             : 
+    5083           0 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5084             : 
+    5085           0 :   if (!isPointInSafetyArea3d(transformed_reference)) {
+    5086           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the point is outside of the safety area!");
+    5087           0 :     res.message = "the point is outside of the safety area";
+    5088           0 :     res.success = false;
+    5089           0 :     return true;
+    5090             :   }
+    5091             : 
+    5092           0 :   if (last_tracker_cmd) {
+    5093             : 
+    5094           0 :     mrs_msgs::ReferenceStamped from_point;
+    5095           0 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5096           0 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5097           0 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5098           0 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5099             : 
+    5100           0 :     if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5101           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the path is going outside the safety area!");
+    5102           0 :       res.message = "the path is going outside the safety area";
+    5103           0 :       res.success = false;
+    5104           0 :       return true;
+    5105             :     }
+    5106             :   }
+    5107             : 
+    5108           0 :   res.message = "the reference is ok";
+    5109           0 :   res.success = true;
+    5110           0 :   return true;
+    5111             : }
+    5112             : 
+    5113             : //}
+    5114             : 
+    5115             : /* //{ callbackValidateReference2d() */
+    5116             : 
+    5117        9514 : bool ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res) {
+    5118             : 
+    5119        9514 :   if (!is_initialized_) {
+    5120           0 :     res.message = "not initialized";
+    5121           0 :     res.success = false;
+    5122           0 :     return true;
+    5123             :   }
+    5124             : 
+    5125        9514 :   if (!validateReference(req.reference.reference, "ControlManager", "reference_for_validation")) {
+    5126           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: NaN detected in variable 'req.reference'!!!");
+    5127           0 :     res.message = "NaNs/infs in input!";
+    5128           0 :     res.success = false;
+    5129           0 :     return true;
+    5130             :   }
+    5131             : 
+    5132             :   // copy member variables
+    5133       19028 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5134       19028 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5135             : 
+    5136             :   // transform the reference to the current frame
+    5137       19028 :   mrs_msgs::ReferenceStamped original_reference;
+    5138        9514 :   original_reference.header    = req.reference.header;
+    5139        9514 :   original_reference.reference = req.reference.reference;
+    5140             : 
+    5141       19028 :   auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5142             : 
+    5143        9514 :   if (!ret) {
+    5144             : 
+    5145           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the reference could not be transformed");
+    5146           0 :     res.message = "the reference could not be transformed";
+    5147           0 :     res.success = false;
+    5148           0 :     return true;
+    5149             :   }
+    5150             : 
+    5151       19028 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5152             : 
+    5153        9514 :   if (!isPointInSafetyArea2d(transformed_reference)) {
+    5154          77 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the point is outside of the safety area!");
+    5155          77 :     res.message = "the point is outside of the safety area";
+    5156          77 :     res.success = false;
+    5157          77 :     return true;
+    5158             :   }
+    5159             : 
+    5160        9437 :   if (last_tracker_cmd) {
+    5161             : 
+    5162          31 :     mrs_msgs::ReferenceStamped from_point;
+    5163          31 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5164          31 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5165          31 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5166          31 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5167             : 
+    5168          31 :     if (!isPathToPointInSafetyArea2d(from_point, transformed_reference)) {
+    5169           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the path is going outside the safety area!");
+    5170           0 :       res.message = "the path is going outside the safety area";
+    5171           0 :       res.success = false;
+    5172           0 :       return true;
+    5173             :     }
+    5174             :   }
+    5175             : 
+    5176        9437 :   res.message = "the reference is ok";
+    5177        9437 :   res.success = true;
+    5178        9437 :   return true;
+    5179             : }
+    5180             : 
+    5181             : //}
+    5182             : 
+    5183             : /* //{ callbackValidateReferenceList() */
+    5184             : 
+    5185           0 : bool ControlManager::callbackValidateReferenceList(mrs_msgs::ValidateReferenceList::Request& req, mrs_msgs::ValidateReferenceList::Response& res) {
+    5186             : 
+    5187           0 :   if (!is_initialized_) {
+    5188           0 :     res.message = "not initialized";
+    5189           0 :     return false;
+    5190             :   }
+    5191             : 
+    5192             :   // copy member variables
+    5193           0 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5194           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5195             : 
+    5196             :   // get the transformer
+    5197           0 :   auto ret = transformer_->getTransform(uav_state.header.frame_id, req.list.header.frame_id, req.list.header.stamp);
+    5198             : 
+    5199           0 :   if (!ret) {
+    5200             : 
+    5201           0 :     ROS_DEBUG("[ControlManager]: could not find transform for the reference");
+    5202           0 :     res.message = "could not find transform";
+    5203           0 :     return false;
+    5204             :   }
+    5205             : 
+    5206           0 :   geometry_msgs::TransformStamped tf = ret.value();
+    5207             : 
+    5208           0 :   for (int i = 0; i < int(req.list.list.size()); i++) {
+    5209             : 
+    5210           0 :     res.success.push_back(true);
+    5211             : 
+    5212           0 :     mrs_msgs::ReferenceStamped original_reference;
+    5213           0 :     original_reference.header    = req.list.header;
+    5214           0 :     original_reference.reference = req.list.list[i];
+    5215             : 
+    5216           0 :     res.success[i] = validateReference(original_reference.reference, "ControlManager", "reference_list");
+    5217             : 
+    5218           0 :     auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+    5219             : 
+    5220           0 :     if (!ret) {
+    5221             : 
+    5222           0 :       ROS_DEBUG("[ControlManager]: the reference could not be transformed");
+    5223           0 :       res.success[i] = false;
+    5224             :     }
+    5225             : 
+    5226           0 :     mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5227             : 
+    5228           0 :     if (!isPointInSafetyArea3d(transformed_reference)) {
+    5229           0 :       res.success[i] = false;
+    5230             :     }
+    5231             : 
+    5232           0 :     if (last_tracker_cmd) {
+    5233             : 
+    5234           0 :       mrs_msgs::ReferenceStamped from_point;
+    5235           0 :       from_point.header.frame_id      = uav_state.header.frame_id;
+    5236           0 :       from_point.reference.position.x = last_tracker_cmd->position.x;
+    5237           0 :       from_point.reference.position.y = last_tracker_cmd->position.y;
+    5238           0 :       from_point.reference.position.z = last_tracker_cmd->position.z;
+    5239             : 
+    5240           0 :       if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5241           0 :         res.success[i] = false;
+    5242             :       }
+    5243             :     }
+    5244             :   }
+    5245             : 
+    5246           0 :   res.message = "references were checked";
+    5247           0 :   return true;
+    5248             : }
+    5249             : 
+    5250             : //}
+    5251             : 
+    5252             : // | -------------- setpoint topics and services -------------- |
+    5253             : 
+    5254             : /* //{ callbackReferenceService() */
+    5255             : 
+    5256           0 : bool ControlManager::callbackReferenceService(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+    5257             : 
+    5258           0 :   if (!is_initialized_) {
+    5259           0 :     res.message = "not initialized";
+    5260           0 :     res.success = false;
+    5261           0 :     return true;
+    5262             :   }
+    5263             : 
+    5264           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackReferenceService");
+    5265           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5266             : 
+    5267           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5268           0 :   des_reference.header    = req.header;
+    5269           0 :   des_reference.reference = req.reference;
+    5270             : 
+    5271           0 :   auto [success, message] = setReference(des_reference);
+    5272             : 
+    5273           0 :   res.success = success;
+    5274           0 :   res.message = message;
+    5275             : 
+    5276           0 :   return true;
+    5277             : }
+    5278             : 
+    5279             : //}
+    5280             : 
+    5281             : /* //{ callbackReferenceTopic() */
+    5282             : 
+    5283           0 : void ControlManager::callbackReferenceTopic(const mrs_msgs::ReferenceStamped::ConstPtr msg) {
+    5284             : 
+    5285           0 :   if (!is_initialized_)
+    5286           0 :     return;
+    5287             : 
+    5288           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackReferenceTopic");
+    5289           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5290             : 
+    5291           0 :   setReference(*msg);
+    5292             : }
+    5293             : 
+    5294             : //}
+    5295             : 
+    5296             : /* //{ callbackVelocityReferenceService() */
+    5297             : 
+    5298         471 : bool ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrv::Request&  req,
+    5299             :                                                       mrs_msgs::VelocityReferenceStampedSrv::Response& res) {
+    5300             : 
+    5301         471 :   if (!is_initialized_) {
+    5302           0 :     res.message = "not initialized";
+    5303           0 :     res.success = false;
+    5304           0 :     return true;
+    5305             :   }
+    5306             : 
+    5307        1413 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackVelocityReferenceService");
+    5308        1413 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackVelocityReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5309             : 
+    5310         942 :   mrs_msgs::VelocityReferenceStamped des_reference;
+    5311         471 :   des_reference = req.reference;
+    5312             : 
+    5313         471 :   auto [success, message] = setVelocityReference(des_reference);
+    5314             : 
+    5315         471 :   res.success = success;
+    5316         471 :   res.message = message;
+    5317             : 
+    5318         471 :   return true;
+    5319             : }
+    5320             : 
+    5321             : //}
+    5322             : 
+    5323             : /* //{ callbackVelocityReferenceTopic() */
+    5324             : 
+    5325           0 : void ControlManager::callbackVelocityReferenceTopic(const mrs_msgs::VelocityReferenceStamped::ConstPtr msg) {
+    5326             : 
+    5327           0 :   if (!is_initialized_)
+    5328           0 :     return;
+    5329             : 
+    5330           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackVelocityReferenceTopic");
+    5331           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackVelocityReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5332             : 
+    5333           0 :   setVelocityReference(*msg);
+    5334             : }
+    5335             : 
+    5336             : //}
+    5337             : 
+    5338             : /* //{ callbackTrajectoryReferenceService() */
+    5339             : 
+    5340           4 : bool ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrv::Request& req, mrs_msgs::TrajectoryReferenceSrv::Response& res) {
+    5341             : 
+    5342           4 :   if (!is_initialized_) {
+    5343           0 :     res.message = "not initialized";
+    5344           0 :     res.success = false;
+    5345           0 :     return true;
+    5346             :   }
+    5347             : 
+    5348          12 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackTrajectoryReferenceService");
+    5349          12 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackTrajectoryReferenceService", scope_timer_logger_, scope_timer_enabled_);
+    5350             : 
+    5351           8 :   auto [success, message, modified, tracker_names, tracker_successes, tracker_messages] = setTrajectoryReference(req.trajectory);
+    5352             : 
+    5353           4 :   res.success          = success;
+    5354           4 :   res.message          = message;
+    5355           4 :   res.modified         = modified;
+    5356           4 :   res.tracker_names    = tracker_names;
+    5357           4 :   res.tracker_messages = tracker_messages;
+    5358             : 
+    5359          28 :   for (size_t i = 0; i < tracker_successes.size(); i++) {
+    5360          24 :     res.tracker_successes.push_back(tracker_successes[i]);
+    5361             :   }
+    5362             : 
+    5363           4 :   return true;
+    5364             : }
+    5365             : 
+    5366             : //}
+    5367             : 
+    5368             : /* //{ callbackTrajectoryReferenceTopic() */
+    5369             : 
+    5370           0 : void ControlManager::callbackTrajectoryReferenceTopic(const mrs_msgs::TrajectoryReference::ConstPtr msg) {
+    5371             : 
+    5372           0 :   if (!is_initialized_)
+    5373           0 :     return;
+    5374             : 
+    5375           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackTrajectoryReferenceTopic");
+    5376           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackTrajectoryReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+    5377             : 
+    5378           0 :   setTrajectoryReference(*msg);
+    5379             : }
+    5380             : 
+    5381             : //}
+    5382             : 
+    5383             : // | ------------- human-callable "goto" services ------------- |
+    5384             : 
+    5385             : /* //{ callbackGoto() */
+    5386             : 
+    5387          25 : bool ControlManager::callbackGoto(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5388             : 
+    5389          25 :   if (!is_initialized_) {
+    5390           0 :     res.message = "not initialized";
+    5391           0 :     res.success = false;
+    5392           0 :     return true;
+    5393             :   }
+    5394             : 
+    5395          75 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGoto");
+    5396          75 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGoto", scope_timer_logger_, scope_timer_enabled_);
+    5397             : 
+    5398          50 :   mrs_msgs::ReferenceStamped des_reference;
+    5399          25 :   des_reference.header.frame_id      = "";
+    5400          25 :   des_reference.header.stamp         = ros::Time(0);
+    5401          25 :   des_reference.reference.position.x = req.goal[REF_X];
+    5402          25 :   des_reference.reference.position.y = req.goal[REF_Y];
+    5403          25 :   des_reference.reference.position.z = req.goal[REF_Z];
+    5404          25 :   des_reference.reference.heading    = req.goal[REF_HEADING];
+    5405             : 
+    5406          50 :   auto [success, message] = setReference(des_reference);
+    5407             : 
+    5408          25 :   res.success = success;
+    5409          25 :   res.message = message;
+    5410             : 
+    5411          25 :   return true;
+    5412             : }
+    5413             : 
+    5414             : //}
+    5415             : 
+    5416             : /* //{ callbackGotoFcu() */
+    5417             : 
+    5418           0 : bool ControlManager::callbackGotoFcu(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5419             : 
+    5420           0 :   if (!is_initialized_) {
+    5421           0 :     res.message = "not initialized";
+    5422           0 :     res.success = false;
+    5423           0 :     return true;
+    5424             :   }
+    5425             : 
+    5426           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoFcu");
+    5427           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoFcu", scope_timer_logger_, scope_timer_enabled_);
+    5428             : 
+    5429           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5430           0 :   des_reference.header.frame_id      = "fcu_untilted";
+    5431           0 :   des_reference.header.stamp         = ros::Time(0);
+    5432           0 :   des_reference.reference.position.x = req.goal[REF_X];
+    5433           0 :   des_reference.reference.position.y = req.goal[REF_Y];
+    5434           0 :   des_reference.reference.position.z = req.goal[REF_Z];
+    5435           0 :   des_reference.reference.heading    = req.goal[REF_HEADING];
+    5436             : 
+    5437           0 :   auto [success, message] = setReference(des_reference);
+    5438             : 
+    5439           0 :   res.success = success;
+    5440           0 :   res.message = message;
+    5441             : 
+    5442           0 :   return true;
+    5443             : }
+    5444             : 
+    5445             : //}
+    5446             : 
+    5447             : /* //{ callbackGotoRelative() */
+    5448             : 
+    5449          22 : bool ControlManager::callbackGotoRelative(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+    5450             : 
+    5451          22 :   if (!is_initialized_) {
+    5452           0 :     res.message = "not initialized";
+    5453           0 :     res.success = false;
+    5454           0 :     return true;
+    5455             :   }
+    5456             : 
+    5457          66 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoRelative");
+    5458          66 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoRelative", scope_timer_logger_, scope_timer_enabled_);
+    5459             : 
+    5460          44 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5461             : 
+    5462          22 :   if (!last_tracker_cmd) {
+    5463           0 :     res.message = "not flying";
+    5464           0 :     res.success = false;
+    5465           0 :     return true;
+    5466             :   }
+    5467             : 
+    5468          44 :   mrs_msgs::ReferenceStamped des_reference;
+    5469          22 :   des_reference.header.frame_id      = "";
+    5470          22 :   des_reference.header.stamp         = ros::Time(0);
+    5471          22 :   des_reference.reference.position.x = last_tracker_cmd->position.x + req.goal[REF_X];
+    5472          22 :   des_reference.reference.position.y = last_tracker_cmd->position.y + req.goal[REF_Y];
+    5473          22 :   des_reference.reference.position.z = last_tracker_cmd->position.z + req.goal[REF_Z];
+    5474          22 :   des_reference.reference.heading    = last_tracker_cmd->heading + req.goal[REF_HEADING];
+    5475             : 
+    5476          44 :   auto [success, message] = setReference(des_reference);
+    5477             : 
+    5478          22 :   res.success = success;
+    5479          22 :   res.message = message;
+    5480             : 
+    5481          22 :   return true;
+    5482             : }
+    5483             : 
+    5484             : //}
+    5485             : 
+    5486             : /* //{ callbackGotoAltitude() */
+    5487             : 
+    5488           0 : bool ControlManager::callbackGotoAltitude(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5489             : 
+    5490           0 :   if (!is_initialized_) {
+    5491           0 :     res.message = "not initialized";
+    5492           0 :     res.success = false;
+    5493           0 :     return true;
+    5494             :   }
+    5495             : 
+    5496           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackGotoAltitude");
+    5497           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackGotoAltitude", scope_timer_logger_, scope_timer_enabled_);
+    5498             : 
+    5499           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5500             : 
+    5501           0 :   if (!last_tracker_cmd) {
+    5502           0 :     res.message = "not flying";
+    5503           0 :     res.success = false;
+    5504           0 :     return true;
+    5505             :   }
+    5506             : 
+    5507           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5508           0 :   des_reference.header.frame_id      = "";
+    5509           0 :   des_reference.header.stamp         = ros::Time(0);
+    5510           0 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5511           0 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5512           0 :   des_reference.reference.position.z = req.goal;
+    5513           0 :   des_reference.reference.heading    = last_tracker_cmd->heading;
+    5514             : 
+    5515           0 :   auto [success, message] = setReference(des_reference);
+    5516             : 
+    5517           0 :   res.success = success;
+    5518           0 :   res.message = message;
+    5519             : 
+    5520           0 :   return true;
+    5521             : }
+    5522             : 
+    5523             : //}
+    5524             : 
+    5525             : /* //{ callbackSetHeading() */
+    5526             : 
+    5527           0 : bool ControlManager::callbackSetHeading(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5528             : 
+    5529           0 :   if (!is_initialized_) {
+    5530           0 :     res.message = "not initialized";
+    5531           0 :     res.success = false;
+    5532           0 :     return true;
+    5533             :   }
+    5534             : 
+    5535           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackSetHeading");
+    5536           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackSetHeading", scope_timer_logger_, scope_timer_enabled_);
+    5537             : 
+    5538           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5539             : 
+    5540           0 :   if (!last_tracker_cmd) {
+    5541           0 :     res.message = "not flying";
+    5542           0 :     res.success = false;
+    5543           0 :     return true;
+    5544             :   }
+    5545             : 
+    5546           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5547           0 :   des_reference.header.frame_id      = "";
+    5548           0 :   des_reference.header.stamp         = ros::Time(0);
+    5549           0 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5550           0 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5551           0 :   des_reference.reference.position.z = last_tracker_cmd->position.z;
+    5552           0 :   des_reference.reference.heading    = req.goal;
+    5553             : 
+    5554           0 :   auto [success, message] = setReference(des_reference);
+    5555             : 
+    5556           0 :   res.success = success;
+    5557           0 :   res.message = message;
+    5558             : 
+    5559           0 :   return true;
+    5560             : }
+    5561             : 
+    5562             : //}
+    5563             : 
+    5564             : /* //{ callbackSetHeadingRelative() */
+    5565             : 
+    5566           0 : bool ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+    5567             : 
+    5568           0 :   if (!is_initialized_) {
+    5569           0 :     res.message = "not initialized";
+    5570           0 :     res.success = false;
+    5571           0 :     return true;
+    5572             :   }
+    5573             : 
+    5574           0 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("callbackSetHeadingRelative");
+    5575           0 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::callbackSetHeadingRelative", scope_timer_logger_, scope_timer_enabled_);
+    5576             : 
+    5577           0 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5578             : 
+    5579           0 :   if (!last_tracker_cmd) {
+    5580           0 :     res.message = "not flying";
+    5581           0 :     res.success = false;
+    5582           0 :     return true;
+    5583             :   }
+    5584             : 
+    5585           0 :   mrs_msgs::ReferenceStamped des_reference;
+    5586           0 :   des_reference.header.frame_id      = "";
+    5587           0 :   des_reference.header.stamp         = ros::Time(0);
+    5588           0 :   des_reference.reference.position.x = last_tracker_cmd->position.x;
+    5589           0 :   des_reference.reference.position.y = last_tracker_cmd->position.y;
+    5590           0 :   des_reference.reference.position.z = last_tracker_cmd->position.z;
+    5591           0 :   des_reference.reference.heading    = last_tracker_cmd->heading + req.goal;
+    5592             : 
+    5593           0 :   auto [success, message] = setReference(des_reference);
+    5594             : 
+    5595           0 :   res.success = success;
+    5596           0 :   res.message = message;
+    5597             : 
+    5598           0 :   return true;
+    5599             : }
+    5600             : 
+    5601             : //}
+    5602             : 
+    5603             : // --------------------------------------------------------------
+    5604             : // |                          routines                          |
+    5605             : // --------------------------------------------------------------
+    5606             : 
+    5607             : /* setReference() //{ */
+    5608             : 
+    5609          47 : std::tuple<bool, std::string> ControlManager::setReference(const mrs_msgs::ReferenceStamped reference_in) {
+    5610             : 
+    5611          94 :   std::stringstream ss;
+    5612             : 
+    5613          47 :   if (!callbacks_enabled_) {
+    5614           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5615           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5616           0 :     return std::tuple(false, ss.str());
+    5617             :   }
+    5618             : 
+    5619          47 :   if (!validateReference(reference_in.reference, "ControlManager", "reference")) {
+    5620           0 :     ss << "incoming reference is not finite!!!";
+    5621           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5622           0 :     return std::tuple(false, ss.str());
+    5623             :   }
+    5624             : 
+    5625             :   // copy member variables
+    5626          94 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5627          94 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5628             : 
+    5629             :   // transform the reference to the current frame
+    5630          94 :   auto ret = transformer_->transformSingle(reference_in, uav_state.header.frame_id);
+    5631             : 
+    5632          47 :   if (!ret) {
+    5633             : 
+    5634           0 :     ss << "the reference could not be transformed";
+    5635           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5636           0 :     return std::tuple(false, ss.str());
+    5637             :   }
+    5638             : 
+    5639          94 :   mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+    5640             : 
+    5641             :   // safety area check
+    5642          47 :   if (!isPointInSafetyArea3d(transformed_reference)) {
+    5643           0 :     ss << "failed to set the reference, the point is outside of the safety area!";
+    5644           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5645           0 :     return std::tuple(false, ss.str());
+    5646             :   }
+    5647             : 
+    5648          47 :   if (last_tracker_cmd) {
+    5649             : 
+    5650          47 :     mrs_msgs::ReferenceStamped from_point;
+    5651          47 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5652          47 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5653          47 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5654          47 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5655             : 
+    5656          47 :     if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+    5657           0 :       ss << "failed to set the reference, the path is going outside the safety area!";
+    5658           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5659           0 :       return std::tuple(false, ss.str());
+    5660             :     }
+    5661             :   }
+    5662             : 
+    5663          47 :   mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    5664             : 
+    5665             :   // prepare the message for current tracker
+    5666          47 :   mrs_msgs::ReferenceSrvRequest reference_request;
+    5667          47 :   reference_request.reference = transformed_reference.reference;
+    5668             : 
+    5669             :   {
+    5670          94 :     std::scoped_lock lock(mutex_tracker_list_);
+    5671             : 
+    5672         141 :     tracker_response = tracker_list_[active_tracker_idx_]->setReference(
+    5673         141 :         mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(reference_request)));
+    5674             : 
+    5675          47 :     if (tracker_response != mrs_msgs::ReferenceSrvResponse::Ptr()) {
+    5676             : 
+    5677          94 :       return std::tuple(tracker_response->success, tracker_response->message);
+    5678             : 
+    5679             :     } else {
+    5680             : 
+    5681           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'setReference()' function!";
+    5682           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the reference: " << ss.str());
+    5683           0 :       return std::tuple(false, ss.str());
+    5684             :     }
+    5685             :   }
+    5686             : }
+    5687             : 
+    5688             : //}
+    5689             : 
+    5690             : /* setVelocityReference() //{ */
+    5691             : 
+    5692         471 : std::tuple<bool, std::string> ControlManager::setVelocityReference(const mrs_msgs::VelocityReferenceStamped& reference_in) {
+    5693             : 
+    5694         942 :   std::stringstream ss;
+    5695             : 
+    5696         471 :   if (!callbacks_enabled_) {
+    5697           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5698           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5699           0 :     return std::tuple(false, ss.str());
+    5700             :   }
+    5701             : 
+    5702         471 :   if (!validateVelocityReference(reference_in.reference, "ControlManager", "velocity_reference")) {
+    5703           0 :     ss << "velocity command is not valid!";
+    5704           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5705           0 :     return std::tuple(false, ss.str());
+    5706             :   }
+    5707             : 
+    5708             :   {
+    5709         471 :     std::scoped_lock lock(mutex_last_tracker_cmd_);
+    5710             : 
+    5711         471 :     if (!last_tracker_cmd_) {
+    5712           0 :       ss << "could not set velocity command, not flying!";
+    5713           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5714           0 :       return std::tuple(false, ss.str());
+    5715             :     }
+    5716             :   }
+    5717             : 
+    5718             :   // copy member variables
+    5719         942 :   auto uav_state        = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5720         942 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    5721             : 
+    5722             :   // | -- transform the velocity reference to the current frame - |
+    5723             : 
+    5724         942 :   mrs_msgs::VelocityReferenceStamped transformed_reference = reference_in;
+    5725             : 
+    5726         942 :   auto ret = transformer_->getTransform(reference_in.header.frame_id, uav_state.header.frame_id, reference_in.header.stamp);
+    5727             : 
+    5728         942 :   geometry_msgs::TransformStamped tf;
+    5729             : 
+    5730         471 :   if (!ret) {
+    5731           0 :     ss << "could not find tf from " << reference_in.header.frame_id << " to " << uav_state.header.frame_id;
+    5732           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5733           0 :     return std::tuple(false, ss.str());
+    5734             :   } else {
+    5735         471 :     tf = ret.value();
+    5736             :   }
+    5737             : 
+    5738             :   // transform the velocity
+    5739             :   {
+    5740         471 :     geometry_msgs::Vector3Stamped velocity;
+    5741         471 :     velocity.header   = reference_in.header;
+    5742         471 :     velocity.vector.x = reference_in.reference.velocity.x;
+    5743         471 :     velocity.vector.y = reference_in.reference.velocity.y;
+    5744         471 :     velocity.vector.z = reference_in.reference.velocity.z;
+    5745             : 
+    5746         471 :     auto ret = transformer_->transform(velocity, tf);
+    5747             : 
+    5748         471 :     if (!ret) {
+    5749             : 
+    5750           0 :       ss << "the velocity reference could not be transformed";
+    5751           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5752           0 :       return std::tuple(false, ss.str());
+    5753             : 
+    5754             :     } else {
+    5755         471 :       transformed_reference.reference.velocity.x = ret->vector.x;
+    5756         471 :       transformed_reference.reference.velocity.y = ret->vector.y;
+    5757         471 :       transformed_reference.reference.velocity.z = ret->vector.z;
+    5758             :     }
+    5759             :   }
+    5760             : 
+    5761             :   // transform the z and the heading
+    5762             :   {
+    5763         471 :     geometry_msgs::PoseStamped pose;
+    5764         471 :     pose.header           = reference_in.header;
+    5765         471 :     pose.pose.position.x  = 0;
+    5766         471 :     pose.pose.position.y  = 0;
+    5767         471 :     pose.pose.position.z  = reference_in.reference.altitude;
+    5768         471 :     pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, reference_in.reference.heading);
+    5769             : 
+    5770         471 :     auto ret = transformer_->transform(pose, tf);
+    5771             : 
+    5772         471 :     if (!ret) {
+    5773             : 
+    5774           0 :       ss << "the velocity reference could not be transformed";
+    5775           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5776           0 :       return std::tuple(false, ss.str());
+    5777             : 
+    5778             :     } else {
+    5779         471 :       transformed_reference.reference.altitude = ret->pose.position.z;
+    5780         471 :       transformed_reference.reference.heading  = mrs_lib::AttitudeConverter(ret->pose.orientation).getHeading();
+    5781             :     }
+    5782             :   }
+    5783             : 
+    5784             :   // the heading rate doees not need to be transformed
+    5785         471 :   transformed_reference.reference.heading_rate = reference_in.reference.heading_rate;
+    5786             : 
+    5787         471 :   transformed_reference.header.stamp    = tf.header.stamp;
+    5788         471 :   transformed_reference.header.frame_id = transformer_->frame_to(tf);
+    5789             : 
+    5790         942 :   mrs_msgs::ReferenceStamped eqivalent_reference = velocityReferenceToReference(transformed_reference);
+    5791             : 
+    5792         471 :   ROS_DEBUG("[ControlManager]: equivalent reference: %.2f, %.2f, %.2f, %.2f", eqivalent_reference.reference.position.x,
+    5793             :             eqivalent_reference.reference.position.y, eqivalent_reference.reference.position.z, eqivalent_reference.reference.heading);
+    5794             : 
+    5795             :   // safety area check
+    5796         471 :   if (!isPointInSafetyArea3d(eqivalent_reference)) {
+    5797           0 :     ss << "failed to set the reference, the point is outside of the safety area!";
+    5798           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5799           0 :     return std::tuple(false, ss.str());
+    5800             :   }
+    5801             : 
+    5802         471 :   if (last_tracker_cmd) {
+    5803             : 
+    5804         471 :     mrs_msgs::ReferenceStamped from_point;
+    5805         471 :     from_point.header.frame_id      = uav_state.header.frame_id;
+    5806         471 :     from_point.reference.position.x = last_tracker_cmd->position.x;
+    5807         471 :     from_point.reference.position.y = last_tracker_cmd->position.y;
+    5808         471 :     from_point.reference.position.z = last_tracker_cmd->position.z;
+    5809             : 
+    5810         471 :     if (!isPathToPointInSafetyArea3d(from_point, eqivalent_reference)) {
+    5811           0 :       ss << "failed to set the reference, the path is going outside the safety area!";
+    5812           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5813           0 :       return std::tuple(false, ss.str());
+    5814             :     }
+    5815             :   }
+    5816             : 
+    5817         471 :   mrs_msgs::VelocityReferenceSrvResponse::ConstPtr tracker_response;
+    5818             : 
+    5819             :   // prepare the message for current tracker
+    5820         471 :   mrs_msgs::VelocityReferenceSrvRequest reference_request;
+    5821         471 :   reference_request.reference = transformed_reference.reference;
+    5822             : 
+    5823             :   {
+    5824         942 :     std::scoped_lock lock(mutex_tracker_list_);
+    5825             : 
+    5826        1413 :     tracker_response = tracker_list_[active_tracker_idx_]->setVelocityReference(
+    5827        1413 :         mrs_msgs::VelocityReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::VelocityReferenceSrvRequest>(reference_request)));
+    5828             : 
+    5829         471 :     if (tracker_response != mrs_msgs::VelocityReferenceSrvResponse::Ptr()) {
+    5830             : 
+    5831         942 :       return std::tuple(tracker_response->success, tracker_response->message);
+    5832             : 
+    5833             :     } else {
+    5834             : 
+    5835           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'setVelocityReference()' function!";
+    5836           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the velocity reference: " << ss.str());
+    5837           0 :       return std::tuple(false, ss.str());
+    5838             :     }
+    5839             :   }
+    5840             : }
+    5841             : 
+    5842             : //}
+    5843             : 
+    5844             : /* setTrajectoryReference() //{ */
+    5845             : 
+    5846           4 : std::tuple<bool, std::string, bool, std::vector<std::string>, std::vector<bool>, std::vector<std::string>> ControlManager::setTrajectoryReference(
+    5847             :     const mrs_msgs::TrajectoryReference trajectory_in) {
+    5848             : 
+    5849           8 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    5850             : 
+    5851           8 :   std::stringstream ss;
+    5852             : 
+    5853           4 :   if (!callbacks_enabled_) {
+    5854           0 :     ss << "can not set the reference, the callbacks are disabled";
+    5855           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5856           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5857             :   }
+    5858             : 
+    5859             :   /* validate the size and check for NaNs //{ */
+    5860             : 
+    5861             :   // check for the size 0, which is invalid
+    5862           4 :   if (trajectory_in.points.size() == 0) {
+    5863             : 
+    5864           0 :     ss << "can not load trajectory with size 0";
+    5865           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5866           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5867             :   }
+    5868             : 
+    5869         584 :   for (int i = 0; i < int(trajectory_in.points.size()); i++) {
+    5870             : 
+    5871             :     // check the point for NaN/inf
+    5872         580 :     bool valid = validateReference(trajectory_in.points[i], "ControlManager", "trajectory_in.points[]");
+    5873             : 
+    5874         580 :     if (!valid) {
+    5875             : 
+    5876           0 :       ss << "trajectory contains NaNs/infs.";
+    5877           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    5878           0 :       return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    5879             :     }
+    5880             :   }
+    5881             : 
+    5882             :   //}
+    5883             : 
+    5884             :   /* publish the debugging topics of the original trajectory //{ */
+    5885             : 
+    5886             :   {
+    5887             : 
+    5888           8 :     geometry_msgs::PoseArray debug_trajectory_out;
+    5889           4 :     debug_trajectory_out.header = trajectory_in.header;
+    5890             : 
+    5891           4 :     debug_trajectory_out.header.frame_id = transformer_->resolveFrame(debug_trajectory_out.header.frame_id);
+    5892             : 
+    5893           4 :     if (debug_trajectory_out.header.stamp == ros::Time(0)) {
+    5894           2 :       debug_trajectory_out.header.stamp = ros::Time::now();
+    5895             :     }
+    5896             : 
+    5897         580 :     for (int i = 0; i < int(trajectory_in.points.size()) - 1; i++) {
+    5898             : 
+    5899         576 :       geometry_msgs::Pose new_pose;
+    5900             : 
+    5901         576 :       new_pose.position.x = trajectory_in.points[i].position.x;
+    5902         576 :       new_pose.position.y = trajectory_in.points[i].position.y;
+    5903         576 :       new_pose.position.z = trajectory_in.points[i].position.z;
+    5904             : 
+    5905         576 :       new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, trajectory_in.points[i].heading);
+    5906             : 
+    5907         576 :       debug_trajectory_out.poses.push_back(new_pose);
+    5908             :     }
+    5909             : 
+    5910           4 :     pub_debug_original_trajectory_poses_.publish(debug_trajectory_out);
+    5911             : 
+    5912           8 :     visualization_msgs::MarkerArray msg_out;
+    5913             : 
+    5914           8 :     visualization_msgs::Marker marker;
+    5915             : 
+    5916           4 :     marker.header = trajectory_in.header;
+    5917             : 
+    5918           4 :     marker.header.frame_id = transformer_->resolveFrame(marker.header.frame_id);
+    5919             : 
+    5920           4 :     if (marker.header.frame_id == "") {
+    5921           0 :       marker.header.frame_id = uav_state.header.frame_id;
+    5922             :     }
+    5923             : 
+    5924           4 :     if (marker.header.stamp == ros::Time(0)) {
+    5925           2 :       marker.header.stamp = ros::Time::now();
+    5926             :     }
+    5927             : 
+    5928           4 :     marker.type             = visualization_msgs::Marker::LINE_LIST;
+    5929           4 :     marker.color.a          = 1;
+    5930           4 :     marker.scale.x          = 0.05;
+    5931           4 :     marker.color.r          = 0;
+    5932           4 :     marker.color.g          = 1;
+    5933           4 :     marker.color.b          = 0;
+    5934           4 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    5935             : 
+    5936         580 :     for (int i = 0; i < int(trajectory_in.points.size()) - 1; i++) {
+    5937             : 
+    5938         576 :       geometry_msgs::Point point1;
+    5939             : 
+    5940         576 :       point1.x = trajectory_in.points[i].position.x;
+    5941         576 :       point1.y = trajectory_in.points[i].position.y;
+    5942         576 :       point1.z = trajectory_in.points[i].position.z;
+    5943             : 
+    5944         576 :       marker.points.push_back(point1);
+    5945             : 
+    5946         576 :       geometry_msgs::Point point2;
+    5947             : 
+    5948         576 :       point2.x = trajectory_in.points[i + 1].position.x;
+    5949         576 :       point2.y = trajectory_in.points[i + 1].position.y;
+    5950         576 :       point2.z = trajectory_in.points[i + 1].position.z;
+    5951             : 
+    5952         576 :       marker.points.push_back(point2);
+    5953             :     }
+    5954             : 
+    5955           4 :     msg_out.markers.push_back(marker);
+    5956             : 
+    5957           4 :     pub_debug_original_trajectory_markers_.publish(msg_out);
+    5958             :   }
+    5959             : 
+    5960             :   //}
+    5961             : 
+    5962           8 :   mrs_msgs::TrajectoryReference processed_trajectory = trajectory_in;
+    5963             : 
+    5964           4 :   int trajectory_size = int(processed_trajectory.points.size());
+    5965             : 
+    5966           4 :   bool trajectory_modified = false;
+    5967             : 
+    5968             :   /* safety area check //{ */
+    5969             : 
+    5970           4 :   if (use_safety_area_) {
+    5971             : 
+    5972           4 :     int last_valid_idx    = 0;
+    5973           4 :     int first_invalid_idx = -1;
+    5974             : 
+    5975           4 :     double min_z = getMinZ(processed_trajectory.header.frame_id);
+    5976           4 :     double max_z = getMaxZ(processed_trajectory.header.frame_id);
+    5977             : 
+    5978         584 :     for (int i = 0; i < trajectory_size; i++) {
+    5979             : 
+    5980         580 :       if (_snap_trajectory_to_safety_area_) {
+    5981             : 
+    5982             :         // saturate the trajectory to min and max Z
+    5983           0 :         if (processed_trajectory.points[i].position.z < min_z) {
+    5984             : 
+    5985           0 :           processed_trajectory.points[i].position.z = min_z;
+    5986           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory violates the minimum Z!");
+    5987           0 :           trajectory_modified = true;
+    5988             :         }
+    5989             : 
+    5990           0 :         if (processed_trajectory.points[i].position.z > max_z) {
+    5991             : 
+    5992           0 :           processed_trajectory.points[i].position.z = max_z;
+    5993           0 :           ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory violates the maximum Z!");
+    5994           0 :           trajectory_modified = true;
+    5995             :         }
+    5996             :       }
+    5997             : 
+    5998             :       // check the point against the safety area
+    5999         580 :       mrs_msgs::ReferenceStamped des_reference;
+    6000         580 :       des_reference.header    = processed_trajectory.header;
+    6001         580 :       des_reference.reference = processed_trajectory.points[i];
+    6002             : 
+    6003         580 :       if (!isPointInSafetyArea3d(des_reference)) {
+    6004             : 
+    6005           0 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory contains points outside of the safety area!");
+    6006           0 :         trajectory_modified = true;
+    6007             : 
+    6008             :         // the first invalid point
+    6009           0 :         if (first_invalid_idx == -1) {
+    6010             : 
+    6011           0 :           first_invalid_idx = i;
+    6012             : 
+    6013           0 :           last_valid_idx = i - 1;
+    6014             :         }
+    6015             : 
+    6016             :         // the point is ok
+    6017             :       } else {
+    6018             : 
+    6019             :         // we found a point, which is ok, after finding a point which was not ok
+    6020         580 :         if (first_invalid_idx != -1) {
+    6021             : 
+    6022             :           // special case, we had no valid point so far
+    6023           0 :           if (last_valid_idx == -1) {
+    6024             : 
+    6025           0 :             ss << "the trajectory starts outside of the safety area!";
+    6026           0 :             ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6027           0 :             return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6028             : 
+    6029             :             // we have a valid point in the past
+    6030             :           } else {
+    6031             : 
+    6032           0 :             if (!_snap_trajectory_to_safety_area_) {
+    6033           0 :               break;
+    6034             :             }
+    6035             : 
+    6036           0 :             bool interpolation_success = true;
+    6037             : 
+    6038             :             // iterpolate between the last valid point and this new valid point
+    6039           0 :             double angle = atan2((processed_trajectory.points[i].position.y - processed_trajectory.points[last_valid_idx].position.y),
+    6040           0 :                                  (processed_trajectory.points[i].position.x - processed_trajectory.points[last_valid_idx].position.x));
+    6041             : 
+    6042             :             double dist_two_points =
+    6043           0 :                 mrs_lib::geometry::dist(vec2_t(processed_trajectory.points[i].position.x, processed_trajectory.points[i].position.y),
+    6044           0 :                                         vec2_t(processed_trajectory.points[last_valid_idx].position.x, processed_trajectory.points[last_valid_idx].position.y));
+    6045           0 :             double step = dist_two_points / (i - last_valid_idx);
+    6046             : 
+    6047           0 :             for (int j = last_valid_idx; j < i; j++) {
+    6048             : 
+    6049           0 :               mrs_msgs::ReferenceStamped temp_point;
+    6050           0 :               temp_point.header.frame_id      = processed_trajectory.header.frame_id;
+    6051           0 :               temp_point.reference.position.x = processed_trajectory.points[last_valid_idx].position.x + (j - last_valid_idx) * cos(angle) * step;
+    6052           0 :               temp_point.reference.position.y = processed_trajectory.points[last_valid_idx].position.y + (j - last_valid_idx) * sin(angle) * step;
+    6053             : 
+    6054           0 :               if (!isPointInSafetyArea2d(temp_point)) {
+    6055             : 
+    6056           0 :                 interpolation_success = false;
+    6057           0 :                 break;
+    6058             : 
+    6059             :               } else {
+    6060             : 
+    6061           0 :                 processed_trajectory.points[j].position.x = temp_point.reference.position.x;
+    6062           0 :                 processed_trajectory.points[j].position.y = temp_point.reference.position.y;
+    6063             :               }
+    6064             :             }
+    6065             : 
+    6066           0 :             if (!interpolation_success) {
+    6067           0 :               break;
+    6068             :             }
+    6069             :           }
+    6070             : 
+    6071           0 :           first_invalid_idx = -1;
+    6072             :         }
+    6073             :       }
+    6074             :     }
+    6075             : 
+    6076             :     // special case, the trajectory does not end with a valid point
+    6077           4 :     if (first_invalid_idx != -1) {
+    6078             : 
+    6079             :       // super special case, the whole trajectory is invalid
+    6080           0 :       if (first_invalid_idx == 0) {
+    6081             : 
+    6082           0 :         ss << "the whole trajectory is outside of the safety area!";
+    6083           0 :         ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6084           0 :         return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6085             : 
+    6086             :         // there is a good portion of the trajectory in the beginning
+    6087             :       } else {
+    6088             : 
+    6089           0 :         trajectory_size = last_valid_idx + 1;
+    6090           0 :         processed_trajectory.points.resize(trajectory_size);
+    6091           0 :         trajectory_modified = true;
+    6092             :       }
+    6093             :     }
+    6094             :   }
+    6095             : 
+    6096           4 :   if (trajectory_size == 0) {
+    6097             : 
+    6098           0 :     ss << "the trajectory happened to be empty after all the checks! This message should not appear!";
+    6099           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6100           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6101             :   }
+    6102             : 
+    6103             :   //}
+    6104             : 
+    6105             :   /* transform the trajectory to the current control frame //{ */
+    6106             : 
+    6107           4 :   std::optional<geometry_msgs::TransformStamped> tf_traj_state;
+    6108             : 
+    6109           4 :   if (processed_trajectory.header.stamp > ros::Time::now()) {
+    6110           0 :     tf_traj_state = transformer_->getTransform(processed_trajectory.header.frame_id, "", processed_trajectory.header.stamp);
+    6111             :   } else {
+    6112           4 :     tf_traj_state = transformer_->getTransform(processed_trajectory.header.frame_id, "", uav_state_.header.stamp);
+    6113             :   }
+    6114             : 
+    6115           4 :   if (!tf_traj_state) {
+    6116           0 :     ss << "could not create TF transformer for the trajectory";
+    6117           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6118           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6119             :   }
+    6120             : 
+    6121           4 :   processed_trajectory.header.frame_id = transformer_->frame_to(*tf_traj_state);
+    6122             : 
+    6123         584 :   for (int i = 0; i < trajectory_size; i++) {
+    6124             : 
+    6125         580 :     mrs_msgs::ReferenceStamped trajectory_point;
+    6126         580 :     trajectory_point.header    = processed_trajectory.header;
+    6127         580 :     trajectory_point.reference = processed_trajectory.points[i];
+    6128             : 
+    6129         580 :     auto ret = transformer_->transform(trajectory_point, *tf_traj_state);
+    6130             : 
+    6131         580 :     if (!ret) {
+    6132             : 
+    6133           0 :       ss << "trajectory cannnot be transformed";
+    6134           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6135           0 :       return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6136             : 
+    6137             :     } else {
+    6138             : 
+    6139             :       // transform the points in the trajectory to the current frame
+    6140         580 :       processed_trajectory.points[i] = ret.value().reference;
+    6141             :     }
+    6142             :   }
+    6143             : 
+    6144             :   //}
+    6145             : 
+    6146           4 :   mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr response;
+    6147           8 :   mrs_msgs::TrajectoryReferenceSrvRequest            request;
+    6148             : 
+    6149             :   // check for empty trajectory
+    6150           4 :   if (processed_trajectory.points.size() == 0) {
+    6151           0 :     ss << "reference trajectory was processing and it is now empty, this should not happen!";
+    6152           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6153           0 :     return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+    6154             :   }
+    6155             : 
+    6156             :   // prepare the message for current tracker
+    6157           4 :   request.trajectory = processed_trajectory;
+    6158             : 
+    6159             :   bool                     success;
+    6160           8 :   std::string              message;
+    6161             :   bool                     modified;
+    6162           8 :   std::vector<std::string> tracker_names;
+    6163           8 :   std::vector<bool>        tracker_successes;
+    6164           8 :   std::vector<std::string> tracker_messages;
+    6165             : 
+    6166             :   {
+    6167           8 :     std::scoped_lock lock(mutex_tracker_list_);
+    6168             : 
+    6169             :     // set the trajectory to the currently active tracker
+    6170          12 :     response = tracker_list_[active_tracker_idx_]->setTrajectoryReference(
+    6171          12 :         mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::TrajectoryReferenceSrvRequest>(request)));
+    6172             : 
+    6173           4 :     tracker_names.push_back(_tracker_names_[active_tracker_idx_]);
+    6174             : 
+    6175           4 :     if (response != mrs_msgs::TrajectoryReferenceSrvResponse::Ptr()) {
+    6176             : 
+    6177           3 :       success  = response->success;
+    6178           3 :       message  = response->message;
+    6179           3 :       modified = response->modified || trajectory_modified;
+    6180           3 :       tracker_successes.push_back(response->success);
+    6181           3 :       tracker_messages.push_back(response->message);
+    6182             : 
+    6183             :     } else {
+    6184             : 
+    6185           1 :       ss << "the active tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'setTrajectoryReference()' function!";
+    6186           1 :       ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6187             : 
+    6188           1 :       success  = true;
+    6189           1 :       message  = ss.str();
+    6190           1 :       modified = false;
+    6191           1 :       tracker_successes.push_back(false);
+    6192           1 :       tracker_messages.push_back(ss.str());
+    6193             :     }
+    6194             : 
+    6195             :     // set the trajectory to the non-active trackers
+    6196          28 :     for (size_t i = 0; i < tracker_list_.size(); i++) {
+    6197             : 
+    6198          24 :       if (i != active_tracker_idx_) {
+    6199             : 
+    6200          20 :         tracker_names.push_back(_tracker_names_[i]);
+    6201             : 
+    6202          60 :         response = tracker_list_[i]->setTrajectoryReference(
+    6203          60 :             mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::TrajectoryReferenceSrvRequest>(request)));
+    6204             : 
+    6205          20 :         if (response != mrs_msgs::TrajectoryReferenceSrvResponse::Ptr()) {
+    6206             : 
+    6207           1 :           tracker_successes.push_back(response->success);
+    6208           1 :           tracker_messages.push_back(response->message);
+    6209             : 
+    6210           1 :           if (response->success) {
+    6211           2 :             std::stringstream ss;
+    6212           1 :             ss << "trajectory loaded to non-active tracker '" << _tracker_names_[i];
+    6213           1 :             ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    6214             :           }
+    6215             : 
+    6216             :         } else {
+    6217             : 
+    6218          19 :           std::stringstream ss;
+    6219          19 :           ss << "the tracker \"" << _tracker_names_[i] << "\" does not implement setTrajectoryReference()";
+    6220          19 :           tracker_successes.push_back(false);
+    6221          19 :           tracker_messages.push_back(ss.str());
+    6222             :         }
+    6223             :       }
+    6224             :     }
+    6225             :   }
+    6226             : 
+    6227           4 :   return std::tuple(success, message, modified, tracker_names, tracker_successes, tracker_messages);
+    6228             : }
+    6229             : 
+    6230             : //}
+    6231             : 
+    6232             : /* isOffboard() //{ */
+    6233             : 
+    6234          16 : bool ControlManager::isOffboard(void) {
+    6235             : 
+    6236          16 :   if (!sh_hw_api_status_.hasMsg()) {
+    6237           0 :     return false;
+    6238             :   }
+    6239             : 
+    6240          16 :   mrs_msgs::HwApiStatusConstPtr hw_api_status = sh_hw_api_status_.getMsg();
+    6241             : 
+    6242          16 :   return hw_api_status->connected && hw_api_status->offboard;
+    6243             : }
+    6244             : 
+    6245             : //}
+    6246             : 
+    6247             : /* setCallbacks() //{ */
+    6248             : 
+    6249          79 : void ControlManager::setCallbacks(bool in) {
+    6250             : 
+    6251          79 :   callbacks_enabled_ = in;
+    6252             : 
+    6253          79 :   std_srvs::SetBoolRequest req_enable_callbacks;
+    6254          79 :   req_enable_callbacks.data = callbacks_enabled_;
+    6255             : 
+    6256             :   {
+    6257         158 :     std::scoped_lock lock(mutex_tracker_list_);
+    6258             : 
+    6259             :     // set callbacks to all trackers
+    6260         553 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6261         474 :       tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6262             :     }
+    6263             :   }
+    6264          79 : }
+    6265             : 
+    6266             : //}
+    6267             : 
+    6268             : /* publishDiagnostics() //{ */
+    6269             : 
+    6270       15124 : void ControlManager::publishDiagnostics(void) {
+    6271             : 
+    6272       15124 :   if (!is_initialized_) {
+    6273           0 :     return;
+    6274             :   }
+    6275             : 
+    6276       45372 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("publishDiagnostics");
+    6277       45372 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::publishDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+    6278             : 
+    6279       30248 :   std::scoped_lock lock(mutex_diagnostics_);
+    6280             : 
+    6281       30248 :   mrs_msgs::ControlManagerDiagnostics diagnostics_msg;
+    6282             : 
+    6283       15124 :   diagnostics_msg.stamp    = ros::Time::now();
+    6284       15124 :   diagnostics_msg.uav_name = _uav_name_;
+    6285             : 
+    6286       15124 :   diagnostics_msg.desired_uav_state_rate = desired_uav_state_rate_;
+    6287             : 
+    6288       15124 :   diagnostics_msg.output_enabled = output_enabled_;
+    6289             : 
+    6290       15124 :   diagnostics_msg.joystick_active = rc_goto_active_;
+    6291             : 
+    6292             :   {
+    6293       15124 :     std::scoped_lock lock(mutex_tracker_list_, mutex_controller_list_);
+    6294             : 
+    6295       15124 :     diagnostics_msg.flying_normally = isFlyingNormally();
+    6296             :   }
+    6297             : 
+    6298       15124 :   diagnostics_msg.bumper_active = bumper_repulsing_;
+    6299             : 
+    6300             :   // | ----------------- fill the tracker status ---------------- |
+    6301             : 
+    6302             :   {
+    6303       30248 :     std::scoped_lock lock(mutex_tracker_list_);
+    6304             : 
+    6305       15124 :     mrs_msgs::TrackerStatus tracker_status;
+    6306             : 
+    6307       15124 :     diagnostics_msg.active_tracker = _tracker_names_[active_tracker_idx_];
+    6308       15124 :     diagnostics_msg.tracker_status = tracker_list_[active_tracker_idx_]->getStatus();
+    6309             :   }
+    6310             : 
+    6311             :   // | --------------- fill the controller status --------------- |
+    6312             : 
+    6313             :   {
+    6314       30248 :     std::scoped_lock lock(mutex_controller_list_);
+    6315             : 
+    6316       15124 :     mrs_msgs::ControllerStatus controller_status;
+    6317             : 
+    6318       15124 :     diagnostics_msg.active_controller = _controller_names_[active_controller_idx_];
+    6319       15124 :     diagnostics_msg.controller_status = controller_list_[active_controller_idx_]->getStatus();
+    6320             :   }
+    6321             : 
+    6322             :   // | ------------ fill in the available controllers ----------- |
+    6323             : 
+    6324       90744 :   for (int i = 0; i < int(_controller_names_.size()); i++) {
+    6325       75620 :     if ((_controller_names_[i] != _failsafe_controller_name_) && (_controller_names_[i] != _eland_controller_name_)) {
+    6326       45372 :       diagnostics_msg.available_controllers.push_back(_controller_names_[i]);
+    6327       45372 :       diagnostics_msg.human_switchable_controllers.push_back(controllers_.at(_controller_names_[i]).human_switchable);
+    6328             :     }
+    6329             :   }
+    6330             : 
+    6331             :   // | ------------- fill in the available trackers ------------- |
+    6332             : 
+    6333      106100 :   for (int i = 0; i < int(_tracker_names_.size()); i++) {
+    6334       90976 :     if (_tracker_names_[i] != _null_tracker_name_) {
+    6335       75852 :       diagnostics_msg.available_trackers.push_back(_tracker_names_[i]);
+    6336       75852 :       diagnostics_msg.human_switchable_trackers.push_back(trackers_.at(_tracker_names_[i]).human_switchable);
+    6337             :     }
+    6338             :   }
+    6339             : 
+    6340             :   // | ------------------------- publish ------------------------ |
+    6341             : 
+    6342       15124 :   ph_diagnostics_.publish(diagnostics_msg);
+    6343             : }
+    6344             : 
+    6345             : //}
+    6346             : 
+    6347             : /* setConstraintsToTrackers() //{ */
+    6348             : 
+    6349         255 : void ControlManager::setConstraintsToTrackers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6350             : 
+    6351         765 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraintsToTrackers");
+    6352         765 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraintsToTrackers", scope_timer_logger_, scope_timer_enabled_);
+    6353             : 
+    6354         255 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6355             : 
+    6356             :   {
+    6357         510 :     std::scoped_lock lock(mutex_tracker_list_);
+    6358             : 
+    6359             :     // for each tracker
+    6360        1788 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    6361             : 
+    6362             :       // if it is the active one, update and retrieve the command
+    6363        4599 :       response = tracker_list_[i]->setConstraints(
+    6364        4599 :           mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr(std::make_unique<mrs_msgs::DynamicsConstraintsSrvRequest>(constraints)));
+    6365             :     }
+    6366             :   }
+    6367         255 : }
+    6368             : 
+    6369             : //}
+    6370             : 
+    6371             : /* setConstraintsToControllers() //{ */
+    6372             : 
+    6373         319 : void ControlManager::setConstraintsToControllers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6374             : 
+    6375         957 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraintsToControllers");
+    6376         957 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraintsToControllers", scope_timer_logger_, scope_timer_enabled_);
+    6377             : 
+    6378         319 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6379             : 
+    6380             :   {
+    6381         638 :     std::scoped_lock lock(mutex_controller_list_);
+    6382             : 
+    6383             :     // for each controller
+    6384        1914 :     for (int i = 0; i < int(controller_list_.size()); i++) {
+    6385             : 
+    6386             :       // if it is the active one, update and retrieve the command
+    6387        4785 :       response = controller_list_[i]->setConstraints(
+    6388        4785 :           mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr(std::make_unique<mrs_msgs::DynamicsConstraintsSrvRequest>(constraints)));
+    6389             :     }
+    6390             :   }
+    6391         319 : }
+    6392             : 
+    6393             : //}
+    6394             : 
+    6395             : /* setConstraints() //{ */
+    6396             : 
+    6397          82 : void ControlManager::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6398             : 
+    6399         246 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("setConstraints");
+    6400         246 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::setConstraints", scope_timer_logger_, scope_timer_enabled_);
+    6401             : 
+    6402          82 :   mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+    6403             : 
+    6404          82 :   setConstraintsToTrackers(constraints);
+    6405             : 
+    6406          82 :   setConstraintsToControllers(constraints);
+    6407          82 : }
+    6408             : 
+    6409             : //}
+    6410             : 
+    6411             : 
+    6412             : /* enforceControllerConstraints() //{ */
+    6413             : 
+    6414      111083 : std::optional<mrs_msgs::DynamicsConstraintsSrvRequest> ControlManager::enforceControllersConstraints(
+    6415             :     const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+    6416             : 
+    6417             :   // copy member variables
+    6418      222166 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    6419             : 
+    6420      111083 :   if (!last_control_output.control_output || !last_control_output.diagnostics.controller_enforcing_constraints) {
+    6421       93983 :     return {};
+    6422             :   }
+    6423             : 
+    6424       17100 :   bool enforcing = false;
+    6425             : 
+    6426       17100 :   auto constraints_out = constraints;
+    6427             : 
+    6428       34200 :   std::scoped_lock lock(mutex_tracker_list_);
+    6429             : 
+    6430             :   // enforce horizontal speed
+    6431       17100 :   if (last_control_output.diagnostics.horizontal_speed_constraint < constraints.constraints.horizontal_speed) {
+    6432       12562 :     constraints_out.constraints.horizontal_speed = last_control_output.diagnostics.horizontal_speed_constraint;
+    6433             : 
+    6434       12562 :     enforcing = true;
+    6435             :   }
+    6436             : 
+    6437             :   // enforce horizontal acceleration
+    6438       17100 :   if (last_control_output.diagnostics.horizontal_acc_constraint < constraints.constraints.horizontal_acceleration) {
+    6439       15406 :     constraints_out.constraints.horizontal_acceleration = last_control_output.diagnostics.horizontal_acc_constraint;
+    6440             : 
+    6441       15406 :     enforcing = true;
+    6442             :   }
+    6443             : 
+    6444             :   // enforce vertical ascending speed
+    6445       17100 :   if (last_control_output.diagnostics.vertical_asc_speed_constraint < constraints.constraints.vertical_ascending_speed) {
+    6446       12562 :     constraints_out.constraints.vertical_ascending_speed = last_control_output.diagnostics.vertical_asc_speed_constraint;
+    6447             : 
+    6448       12562 :     enforcing = true;
+    6449             :   }
+    6450             : 
+    6451             :   // enforce vertical ascending acceleration
+    6452       17100 :   if (last_control_output.diagnostics.vertical_asc_acc_constraint < constraints.constraints.vertical_ascending_acceleration) {
+    6453           0 :     constraints_out.constraints.vertical_ascending_acceleration = last_control_output.diagnostics.vertical_asc_acc_constraint;
+    6454             : 
+    6455           0 :     enforcing = true;
+    6456             :   }
+    6457             : 
+    6458             :   // enforce vertical descending speed
+    6459       17100 :   if (last_control_output.diagnostics.vertical_desc_speed_constraint < constraints.constraints.vertical_descending_speed) {
+    6460       12562 :     constraints_out.constraints.vertical_descending_speed = last_control_output.diagnostics.vertical_desc_speed_constraint;
+    6461             : 
+    6462       12562 :     enforcing = true;
+    6463             :   }
+    6464             : 
+    6465             :   // enforce vertical descending acceleration
+    6466       17100 :   if (last_control_output.diagnostics.vertical_desc_acc_constraint < constraints.constraints.vertical_descending_acceleration) {
+    6467           0 :     constraints_out.constraints.vertical_descending_acceleration = last_control_output.diagnostics.vertical_desc_acc_constraint;
+    6468             : 
+    6469           0 :     enforcing = true;
+    6470             :   }
+    6471             : 
+    6472       17100 :   if (enforcing) {
+    6473       15406 :     return {constraints_out};
+    6474             :   } else {
+    6475        1694 :     return {};
+    6476             :   }
+    6477             : }
+    6478             : 
+    6479             : //}
+    6480             : 
+    6481             : /* isFlyingNormally() //{ */
+    6482             : 
+    6483       15431 : bool ControlManager::isFlyingNormally(void) {
+    6484             : 
+    6485       13104 :   return callbacks_enabled_ && (output_enabled_) && (offboard_mode_) && (armed_) &&
+    6486        8169 :          (((active_tracker_idx_ != _ehover_tracker_idx_) && (active_controller_idx_ != _eland_controller_idx_) &&
+    6487        8169 :            (active_controller_idx_ != _failsafe_controller_idx_)) ||
+    6488       30628 :           _controller_names_.size() == 1) &&
+    6489       21507 :          (((active_tracker_idx_ != _null_tracker_idx_) && (active_tracker_idx_ != _landoff_tracker_idx_)) || _tracker_names_.size() == 1);
+    6490             : }
+    6491             : 
+    6492             : //}
+    6493             : 
+    6494             : /* //{ getMass() */
+    6495             : 
+    6496         425 : double ControlManager::getMass(void) {
+    6497             : 
+    6498         850 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    6499             : 
+    6500         425 :   if (last_control_output.diagnostics.mass_estimator) {
+    6501          13 :     return _uav_mass_ + last_control_output.diagnostics.mass_difference;
+    6502             :   } else {
+    6503         412 :     return _uav_mass_;
+    6504             :   }
+    6505             : }
+    6506             : 
+    6507             : //}
+    6508             : 
+    6509             : /* loadConfigFile() //{ */
+    6510             : 
+    6511           0 : bool ControlManager::loadConfigFile(const std::string& file_path, const std::string ns) {
+    6512             : 
+    6513           0 :   const std::string name_space = nh_.getNamespace() + "/" + ns;
+    6514             : 
+    6515           0 :   ROS_INFO("[ControlManager]: loading '%s' under the namespace '%s'", file_path.c_str(), name_space.c_str());
+    6516             : 
+    6517             :   // load the user-requested file
+    6518             :   {
+    6519           0 :     std::string command = "rosparam load " + file_path + " " + name_space;
+    6520           0 :     int         result  = std::system(command.c_str());
+    6521             : 
+    6522           0 :     if (result != 0) {
+    6523           0 :       ROS_ERROR("[ControlManager]: failed to load '%s'", file_path.c_str());
+    6524           0 :       return false;
+    6525             :     }
+    6526             :   }
+    6527             : 
+    6528             :   // load the platform config
+    6529           0 :   if (_platform_config_ != "") {
+    6530           0 :     std::string command = "rosparam load " + _platform_config_ + " " + name_space;
+    6531           0 :     int         result  = std::system(command.c_str());
+    6532             : 
+    6533           0 :     if (result != 0) {
+    6534           0 :       ROS_ERROR("[ControlManager]: failed to load the platform config file '%s'", _platform_config_.c_str());
+    6535           0 :       return false;
+    6536             :     }
+    6537             :   }
+    6538             : 
+    6539             :   // load the custom config
+    6540           0 :   if (_custom_config_ != "") {
+    6541           0 :     std::string command = "rosparam load " + _custom_config_ + " " + name_space;
+    6542           0 :     int         result  = std::system(command.c_str());
+    6543             : 
+    6544           0 :     if (result != 0) {
+    6545           0 :       ROS_ERROR("[ControlManager]: failed to load the custom config file '%s'", _custom_config_.c_str());
+    6546           0 :       return false;
+    6547             :     }
+    6548             :   }
+    6549             : 
+    6550           0 :   return true;
+    6551             : }
+    6552             : 
+    6553             : //}
+    6554             : 
+    6555             : // | ----------------------- safety area ---------------------- |
+    6556             : 
+    6557             : /* //{ isPointInSafetyArea3d() */
+    6558             : 
+    6559        1160 : bool ControlManager::isPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& point) {
+    6560             : 
+    6561        1160 :   if (!use_safety_area_) {
+    6562         487 :     return true;
+    6563             :   }
+    6564             : 
+    6565        1346 :   auto tfed_horizontal = transformer_->transformSingle(point, _safety_area_horizontal_frame_);
+    6566             : 
+    6567         673 :   if (!tfed_horizontal) {
+    6568           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform the point to the safety area horizontal frame");
+    6569           0 :     return false;
+    6570             :   }
+    6571             : 
+    6572         673 :   if (!safety_zone_->isPointValid(tfed_horizontal->reference.position.x, tfed_horizontal->reference.position.y)) {
+    6573           0 :     return false;
+    6574             :   }
+    6575             : 
+    6576         673 :   if (point.reference.position.z < getMinZ(point.header.frame_id) || point.reference.position.z > getMaxZ(point.header.frame_id)) {
+    6577           0 :     return false;
+    6578             :   }
+    6579             : 
+    6580         673 :   return true;
+    6581             : }
+    6582             : 
+    6583             : //}
+    6584             : 
+    6585             : /* //{ isPointInSafetyArea2d() */
+    6586             : 
+    6587        9660 : bool ControlManager::isPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& point) {
+    6588             : 
+    6589        9660 :   if (!use_safety_area_) {
+    6590         596 :     return true;
+    6591             :   }
+    6592             : 
+    6593       18128 :   auto tfed_horizontal = transformer_->transformSingle(point, _safety_area_horizontal_frame_);
+    6594             : 
+    6595        9064 :   if (!tfed_horizontal) {
+    6596           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform the point to the safety area horizontal frame");
+    6597           0 :     return false;
+    6598             :   }
+    6599             : 
+    6600        9064 :   if (!safety_zone_->isPointValid(tfed_horizontal->reference.position.x, tfed_horizontal->reference.position.y)) {
+    6601          78 :     return false;
+    6602             :   }
+    6603             : 
+    6604        8986 :   return true;
+    6605             : }
+    6606             : 
+    6607             : //}
+    6608             : 
+    6609             : /* //{ isPathToPointInSafetyArea3d() */
+    6610             : 
+    6611         518 : bool ControlManager::isPathToPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& start, const mrs_msgs::ReferenceStamped& end) {
+    6612             : 
+    6613         518 :   if (!use_safety_area_) {
+    6614         487 :     return true;
+    6615             :   }
+    6616             : 
+    6617          31 :   if (!isPointInSafetyArea3d(start) || !isPointInSafetyArea3d(end)) {
+    6618           0 :     return false;
+    6619             :   }
+    6620             : 
+    6621          62 :   mrs_msgs::ReferenceStamped start_transformed, end_transformed;
+    6622             : 
+    6623             :   {
+    6624          31 :     auto ret = transformer_->transformSingle(start, _safety_area_horizontal_frame_);
+    6625             : 
+    6626          31 :     if (!ret) {
+    6627             : 
+    6628           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6629             : 
+    6630           0 :       return false;
+    6631             :     }
+    6632             : 
+    6633          31 :     start_transformed = ret.value();
+    6634             :   }
+    6635             : 
+    6636             :   {
+    6637          31 :     auto ret = transformer_->transformSingle(end, _safety_area_horizontal_frame_);
+    6638             : 
+    6639          31 :     if (!ret) {
+    6640             : 
+    6641           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6642             : 
+    6643           0 :       return false;
+    6644             :     }
+    6645             : 
+    6646          31 :     end_transformed = ret.value();
+    6647             :   }
+    6648             : 
+    6649          31 :   return safety_zone_->isPathValid(start_transformed.reference.position.x, start_transformed.reference.position.y, end_transformed.reference.position.x,
+    6650          31 :                                    end_transformed.reference.position.y);
+    6651             : }
+    6652             : 
+    6653             : //}
+    6654             : 
+    6655             : /* //{ isPathToPointInSafetyArea2d() */
+    6656             : 
+    6657          31 : bool ControlManager::isPathToPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& start, const mrs_msgs::ReferenceStamped& end) {
+    6658             : 
+    6659          31 :   if (!use_safety_area_) {
+    6660           0 :     return true;
+    6661             :   }
+    6662             : 
+    6663          62 :   mrs_msgs::ReferenceStamped start_transformed, end_transformed;
+    6664             : 
+    6665          31 :   if (!isPointInSafetyArea2d(start) || !isPointInSafetyArea2d(end)) {
+    6666           0 :     return false;
+    6667             :   }
+    6668             : 
+    6669             :   {
+    6670          31 :     auto ret = transformer_->transformSingle(start, _safety_area_horizontal_frame_);
+    6671             : 
+    6672          31 :     if (!ret) {
+    6673             : 
+    6674           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6675             : 
+    6676           0 :       return false;
+    6677             :     }
+    6678             : 
+    6679          31 :     start_transformed = ret.value();
+    6680             :   }
+    6681             : 
+    6682             :   {
+    6683          31 :     auto ret = transformer_->transformSingle(end, _safety_area_horizontal_frame_);
+    6684             : 
+    6685          31 :     if (!ret) {
+    6686             : 
+    6687           0 :       ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+    6688             : 
+    6689           0 :       return false;
+    6690             :     }
+    6691             : 
+    6692          31 :     end_transformed = ret.value();
+    6693             :   }
+    6694             : 
+    6695          31 :   return safety_zone_->isPathValid(start_transformed.reference.position.x, start_transformed.reference.position.y, end_transformed.reference.position.x,
+    6696          31 :                                    end_transformed.reference.position.y);
+    6697             : }
+    6698             : 
+    6699             : //}
+    6700             : 
+    6701             : /* //{ getMaxZ() */
+    6702             : 
+    6703       10770 : double ControlManager::getMaxZ(const std::string& frame_id) {
+    6704             : 
+    6705             :   // | ------- first, calculate max_z from the safety area ------ |
+    6706             : 
+    6707       10770 :   double safety_area_max_z = std::numeric_limits<float>::max();
+    6708             : 
+    6709             :   {
+    6710             : 
+    6711       21540 :     geometry_msgs::PointStamped point;
+    6712             : 
+    6713       10770 :     point.header.frame_id = _safety_area_vertical_frame_;
+    6714       10770 :     point.point.x         = 0;
+    6715       10770 :     point.point.y         = 0;
+    6716       10770 :     point.point.z         = _safety_area_max_z_;
+    6717             : 
+    6718       10770 :     auto ret = transformer_->transformSingle(point, frame_id);
+    6719             : 
+    6720       10770 :     if (!ret) {
+    6721           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform safety area's max_z to '%s'", frame_id.c_str());
+    6722             :     }
+    6723             : 
+    6724       10770 :     safety_area_max_z = ret->point.z;
+    6725             :   }
+    6726             : 
+    6727             :   // | ------------ overwrite from estimation manager ----------- |
+    6728             : 
+    6729       10770 :   double estimation_manager_max_z = std::numeric_limits<float>::max();
+    6730             : 
+    6731             :   {
+    6732             :     // if possible, override it with max z from the estimation manager
+    6733       10770 :     if (sh_max_z_.hasMsg()) {
+    6734             : 
+    6735       21520 :       auto msg = sh_max_z_.getMsg();
+    6736             : 
+    6737             :       // transform it into the safety area frame
+    6738       21520 :       geometry_msgs::PointStamped point;
+    6739       10760 :       point.header  = msg->header;
+    6740       10760 :       point.point.x = 0;
+    6741       10760 :       point.point.y = 0;
+    6742       10760 :       point.point.z = msg->value;
+    6743             : 
+    6744       10760 :       auto ret = transformer_->transformSingle(point, frame_id);
+    6745             : 
+    6746       10760 :       if (!ret) {
+    6747           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform estimation manager's max_z to the current control frame");
+    6748             :       }
+    6749             : 
+    6750       10760 :       estimation_manager_max_z = ret->point.z;
+    6751             :     }
+    6752             :   }
+    6753             : 
+    6754       10770 :   if (estimation_manager_max_z < safety_area_max_z) {
+    6755       10176 :     return estimation_manager_max_z;
+    6756             :   } else {
+    6757         594 :     return safety_area_max_z;
+    6758             :   }
+    6759             : }
+    6760             : 
+    6761             : //}
+    6762             : 
+    6763             : /* //{ getMinZ() */
+    6764             : 
+    6765       10770 : double ControlManager::getMinZ(const std::string& frame_id) {
+    6766             : 
+    6767       10770 :   if (!use_safety_area_) {
+    6768           0 :     return std::numeric_limits<double>::lowest();
+    6769             :   }
+    6770             : 
+    6771       21540 :   geometry_msgs::PointStamped point;
+    6772             : 
+    6773       10770 :   point.header.frame_id = _safety_area_vertical_frame_;
+    6774       10770 :   point.point.x         = 0;
+    6775       10770 :   point.point.y         = 0;
+    6776       10770 :   point.point.z         = _safety_area_min_z_;
+    6777             : 
+    6778       21540 :   auto ret = transformer_->transformSingle(point, frame_id);
+    6779             : 
+    6780       10770 :   if (!ret) {
+    6781           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform safety area's min_z to '%s'", frame_id.c_str());
+    6782           0 :     return std::numeric_limits<double>::lowest();
+    6783             :   }
+    6784             : 
+    6785       10770 :   return ret->point.z;
+    6786             : }
+    6787             : 
+    6788             : //}
+    6789             : 
+    6790             : // | --------------------- obstacle bumper -------------------- |
+    6791             : 
+    6792             : /* bumperPushFromObstacle() //{ */
+    6793             : 
+    6794         301 : void ControlManager::bumperPushFromObstacle(void) {
+    6795             : 
+    6796             :   // | --------------- fabricate the min distances -------------- |
+    6797             : 
+    6798         301 :   double min_distance_horizontal = _bumper_horizontal_distance_;
+    6799         301 :   double min_distance_vertical   = _bumper_vertical_distance_;
+    6800             : 
+    6801         301 :   if (_bumper_horizontal_derive_from_dynamics_ || _bumper_vertical_derive_from_dynamics_) {
+    6802             : 
+    6803         301 :     auto constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    6804             : 
+    6805         301 :     if (_bumper_horizontal_derive_from_dynamics_) {
+    6806             : 
+    6807         301 :       const double horizontal_t_stop    = constraints.constraints.horizontal_speed / constraints.constraints.horizontal_acceleration;
+    6808         301 :       const double horizontal_stop_dist = (horizontal_t_stop * constraints.constraints.horizontal_speed) / 2.0;
+    6809             : 
+    6810         301 :       min_distance_horizontal += 1.5 * horizontal_stop_dist;
+    6811             :     }
+    6812             : 
+    6813         301 :     if (_bumper_vertical_derive_from_dynamics_) {
+    6814             : 
+    6815             : 
+    6816             :       // larger from the two accelerations
+    6817         602 :       const double vert_acc = constraints.constraints.vertical_ascending_acceleration > constraints.constraints.vertical_descending_acceleration
+    6818         301 :                                   ? constraints.constraints.vertical_ascending_acceleration
+    6819             :                                   : constraints.constraints.vertical_descending_acceleration;
+    6820             : 
+    6821             :       // larger from the two speeds
+    6822         602 :       const double vert_speed = constraints.constraints.vertical_ascending_speed > constraints.constraints.vertical_descending_speed
+    6823         301 :                                     ? constraints.constraints.vertical_ascending_speed
+    6824             :                                     : constraints.constraints.vertical_descending_speed;
+    6825             : 
+    6826         301 :       const double vertical_t_stop    = vert_speed / vert_acc;
+    6827         301 :       const double vertical_stop_dist = (vertical_t_stop * vert_speed) / 2.0;
+    6828             : 
+    6829         301 :       min_distance_vertical += 1.5 * vertical_stop_dist;
+    6830             :     }
+    6831             :   }
+    6832             : 
+    6833             :   // | ----------------------------  ---------------------------- |
+    6834             : 
+    6835             :   // copy member variables
+    6836         301 :   mrs_msgs::ObstacleSectorsConstPtr bumper_data = sh_bumper_.getMsg();
+    6837         301 :   auto                              uav_state   = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    6838             : 
+    6839         301 :   double sector_size = TAU / double(bumper_data->n_horizontal_sectors);
+    6840             : 
+    6841         301 :   double direction          = 0;
+    6842         301 :   double repulsion_distance = std::numeric_limits<double>::max();
+    6843             : 
+    6844         301 :   bool horizontal_collision_detected = false;
+    6845         301 :   bool vertical_collision_detected   = false;
+    6846             : 
+    6847         301 :   double min_horizontal_sector_distance = std::numeric_limits<double>::max();
+    6848         301 :   size_t min_sector_id                  = 0;
+    6849             : 
+    6850        2709 :   for (unsigned long i = 0; i < bumper_data->n_horizontal_sectors; i++) {
+    6851             : 
+    6852        2408 :     if (bumper_data->sectors[i] < 0) {
+    6853           0 :       continue;
+    6854             :     }
+    6855             : 
+    6856        2408 :     if (bumper_data->sectors[i] < min_horizontal_sector_distance) {
+    6857         301 :       min_horizontal_sector_distance = bumper_data->sectors[i];
+    6858         301 :       min_sector_id                  = i;
+    6859             :     }
+    6860             :   }
+    6861             : 
+    6862             :   // if the sector is under the threshold distance
+    6863         301 :   if (min_horizontal_sector_distance < min_distance_horizontal) {
+    6864             : 
+    6865             :     // get the desired direction of motion
+    6866         120 :     double oposite_direction  = double(min_sector_id) * sector_size + M_PI;
+    6867         120 :     int    oposite_sector_idx = bumperGetSectorId(cos(oposite_direction), sin(oposite_direction), 0);
+    6868             : 
+    6869             :     // get the id of the oposite sector
+    6870         120 :     direction = oposite_direction;
+    6871             : 
+    6872         120 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: found potential collision (sector %lu vs. %d), obstacle distance: %.2f, repulsing", min_sector_id,
+    6873             :                       oposite_sector_idx, bumper_data->sectors[min_sector_id]);
+    6874             : 
+    6875         120 :     repulsion_distance = min_distance_horizontal + _bumper_horizontal_overshoot_ - bumper_data->sectors[min_sector_id];
+    6876             : 
+    6877         120 :     horizontal_collision_detected = true;
+    6878             :   }
+    6879             : 
+    6880         301 :   double vertical_repulsion_distance = 0;
+    6881             : 
+    6882             :   // check for vertical collision down
+    6883         301 :   if (bumper_data->sectors[bumper_data->n_horizontal_sectors] > 0 && bumper_data->sectors[bumper_data->n_horizontal_sectors] <= min_distance_vertical) {
+    6884             : 
+    6885           0 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: potential collision below");
+    6886           0 :     vertical_collision_detected = true;
+    6887           0 :     vertical_repulsion_distance = min_distance_vertical - bumper_data->sectors[bumper_data->n_horizontal_sectors];
+    6888             :   }
+    6889             : 
+    6890             :   // check for vertical collision up
+    6891         301 :   if (bumper_data->sectors[bumper_data->n_horizontal_sectors + 1] > 0 && bumper_data->sectors[bumper_data->n_horizontal_sectors + 1] <= min_distance_vertical) {
+    6892             : 
+    6893           0 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: potential collision above");
+    6894           0 :     vertical_collision_detected = true;
+    6895           0 :     vertical_repulsion_distance = -(min_distance_vertical - bumper_data->sectors[bumper_data->n_horizontal_sectors + 1]);
+    6896             :   }
+    6897             : 
+    6898             :   // if potential collision was detected and we should start the repulsing_
+    6899         301 :   if (horizontal_collision_detected || vertical_collision_detected) {
+    6900             : 
+    6901         120 :     if (!bumper_repulsing_) {
+    6902             : 
+    6903           1 :       if (_bumper_switch_tracker_) {
+    6904             : 
+    6905           1 :         auto        active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    6906           2 :         std::string active_tracker_name = _tracker_names_[active_tracker_idx];
+    6907             : 
+    6908             :         // remember the previously active tracker
+    6909           1 :         bumper_previous_tracker_ = active_tracker_name;
+    6910             : 
+    6911           1 :         if (active_tracker_name != _bumper_tracker_name_) {
+    6912             : 
+    6913           0 :           switchTracker(_bumper_tracker_name_);
+    6914             :         }
+    6915             :       }
+    6916             : 
+    6917           1 :       if (_bumper_switch_controller_) {
+    6918             : 
+    6919           1 :         auto        active_controller_idx  = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    6920           2 :         std::string active_controller_name = _controller_names_[active_controller_idx];
+    6921             : 
+    6922             :         // remember the previously active controller
+    6923           1 :         bumper_previous_controller_ = active_controller_name;
+    6924             : 
+    6925           1 :         if (active_controller_name != _bumper_controller_name_) {
+    6926             : 
+    6927           1 :           switchController(_bumper_controller_name_);
+    6928             :         }
+    6929             :       }
+    6930             :     }
+    6931             : 
+    6932         120 :     bumper_repulsing_ = true;
+    6933             : 
+    6934         120 :     callbacks_enabled_ = false;
+    6935             : 
+    6936           0 :     mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+    6937             : 
+    6938         120 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    6939             : 
+    6940             :     // create the reference in the fcu_untilted frame
+    6941         120 :     mrs_msgs::ReferenceStamped reference_fcu_untilted;
+    6942             : 
+    6943         120 :     reference_fcu_untilted.header.frame_id = "fcu_untilted";
+    6944             : 
+    6945         120 :     if (horizontal_collision_detected) {
+    6946         120 :       reference_fcu_untilted.reference.position.x = cos(direction) * repulsion_distance;
+    6947         120 :       reference_fcu_untilted.reference.position.y = sin(direction) * repulsion_distance;
+    6948             :     } else {
+    6949           0 :       reference_fcu_untilted.reference.position.x = 0;
+    6950           0 :       reference_fcu_untilted.reference.position.y = 0;
+    6951             :     }
+    6952             : 
+    6953         120 :     reference_fcu_untilted.reference.heading = 0;
+    6954             : 
+    6955         120 :     if (vertical_collision_detected) {
+    6956           0 :       reference_fcu_untilted.reference.position.z = vertical_repulsion_distance;
+    6957             :     } else {
+    6958         120 :       reference_fcu_untilted.reference.position.z = 0;
+    6959             :     }
+    6960             : 
+    6961             :     {
+    6962         120 :       std::scoped_lock lock(mutex_tracker_list_);
+    6963             : 
+    6964             :       // transform the reference into the currently used frame
+    6965             :       // this is under the mutex_tracker_list since we don't won't the odometry switch to happen
+    6966             :       // to the tracker before we actually call the goto service
+    6967             : 
+    6968         120 :       auto ret = transformer_->transformSingle(reference_fcu_untilted, uav_state.header.frame_id);
+    6969             : 
+    6970         120 :       if (!ret) {
+    6971           0 :         ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: bumper reference could not be transformed");
+    6972           0 :         return;
+    6973             :       }
+    6974             : 
+    6975         120 :       reference_fcu_untilted = ret.value();
+    6976             : 
+    6977             :       // copy the reference into the service type message
+    6978         120 :       mrs_msgs::ReferenceSrvRequest req_goto_out;
+    6979         120 :       req_goto_out.reference = reference_fcu_untilted.reference;
+    6980             : 
+    6981             :       // disable callbacks of all trackers
+    6982         120 :       req_enable_callbacks.data = false;
+    6983         840 :       for (size_t i = 0; i < tracker_list_.size(); i++) {
+    6984         720 :         tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6985             :       }
+    6986             : 
+    6987             :       // enable the callbacks for the active tracker
+    6988         120 :       req_enable_callbacks.data = true;
+    6989         120 :       tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6990             : 
+    6991             :       // call the goto
+    6992         360 :       tracker_response = tracker_list_[active_tracker_idx_]->setReference(
+    6993         360 :           mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(req_goto_out)));
+    6994             : 
+    6995             :       // disable the callbacks back again
+    6996         120 :       req_enable_callbacks.data = false;
+    6997         120 :       tracker_list_[active_tracker_idx_]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    6998             :     }
+    6999             :   }
+    7000             : 
+    7001             :   // if repulsing_ and the distance is safe once again
+    7002         301 :   if (bumper_repulsing_ && !horizontal_collision_detected && !vertical_collision_detected) {
+    7003             : 
+    7004           1 :     ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: no more collision, stopping repulsion");
+    7005             : 
+    7006           1 :     if (_bumper_switch_tracker_) {
+    7007             : 
+    7008           1 :       auto        active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7009           2 :       std::string active_tracker_name = _tracker_names_[active_tracker_idx];
+    7010             : 
+    7011           1 :       if (active_tracker_name != bumper_previous_tracker_) {
+    7012             : 
+    7013           0 :         switchTracker(bumper_previous_tracker_);
+    7014             :       }
+    7015             :     }
+    7016             : 
+    7017           1 :     if (_bumper_switch_controller_) {
+    7018             : 
+    7019           1 :       auto        active_controller_idx  = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7020           2 :       std::string active_controller_name = _controller_names_[active_controller_idx];
+    7021             : 
+    7022           1 :       if (active_controller_name != bumper_previous_controller_) {
+    7023             : 
+    7024           1 :         switchController(bumper_previous_controller_);
+    7025             :       }
+    7026             :     }
+    7027             : 
+    7028           1 :     std_srvs::SetBoolRequest req_enable_callbacks;
+    7029             : 
+    7030             :     {
+    7031           2 :       std::scoped_lock lock(mutex_tracker_list_);
+    7032             : 
+    7033             :       // enable callbacks of all trackers
+    7034           1 :       req_enable_callbacks.data = true;
+    7035           7 :       for (size_t i = 0; i < tracker_list_.size(); i++) {
+    7036           6 :         tracker_list_[i]->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+    7037             :       }
+    7038             :     }
+    7039             : 
+    7040           1 :     callbacks_enabled_ = true;
+    7041             : 
+    7042           1 :     bumper_repulsing_ = false;
+    7043             :   }
+    7044             : }
+    7045             : 
+    7046             : //}
+    7047             : 
+    7048             : /* bumperGetSectorId() //{ */
+    7049             : 
+    7050         120 : int ControlManager::bumperGetSectorId(const double& x, const double& y, [[maybe_unused]] const double& z) {
+    7051             : 
+    7052             :   // copy member variables
+    7053         120 :   mrs_msgs::ObstacleSectorsConstPtr bumper_data = sh_bumper_.getMsg();
+    7054             : 
+    7055             :   // heading of the point in drone frame
+    7056         120 :   double point_heading_horizontal = atan2(y, x);
+    7057             : 
+    7058         120 :   point_heading_horizontal += TAU;
+    7059             : 
+    7060             :   // if point_heading_horizontal is greater then 2*M_PI mod it
+    7061         120 :   if (fabs(point_heading_horizontal) >= TAU) {
+    7062         120 :     point_heading_horizontal = fmod(point_heading_horizontal, TAU);
+    7063             :   }
+    7064             : 
+    7065             :   // heading of the right edge of the first sector
+    7066         120 :   double sector_size = TAU / double(bumper_data->n_horizontal_sectors);
+    7067             : 
+    7068             :   // calculate the idx
+    7069         120 :   int idx = floor((point_heading_horizontal + (sector_size / 2.0)) / sector_size);
+    7070             : 
+    7071         120 :   if (idx > int(bumper_data->n_horizontal_sectors) - 1) {
+    7072           0 :     idx -= bumper_data->n_horizontal_sectors;
+    7073             :   }
+    7074             : 
+    7075         240 :   return idx;
+    7076             : }
+    7077             : 
+    7078             : //}
+    7079             : 
+    7080             : // | ------------------------- safety ------------------------- |
+    7081             : 
+    7082             : /* //{ changeLandingState() */
+    7083             : 
+    7084          10 : void ControlManager::changeLandingState(LandingStates_t new_state) {
+    7085             : 
+    7086             :   // copy member variables
+    7087          20 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7088             : 
+    7089             :   {
+    7090          10 :     std::scoped_lock lock(mutex_landing_state_machine_);
+    7091             : 
+    7092          10 :     previous_state_landing_ = current_state_landing_;
+    7093          10 :     current_state_landing_  = new_state;
+    7094             :   }
+    7095             : 
+    7096          10 :   switch (current_state_landing_) {
+    7097             : 
+    7098           3 :     case IDLE_STATE:
+    7099           3 :       break;
+    7100           7 :     case LANDING_STATE: {
+    7101             : 
+    7102           7 :       ROS_DEBUG("[ControlManager]: starting eland timer");
+    7103           7 :       timer_eland_.start();
+    7104           7 :       ROS_DEBUG("[ControlManager]: eland timer started");
+    7105           7 :       eland_triggered_ = true;
+    7106           7 :       bumper_enabled_  = false;
+    7107             : 
+    7108           7 :       landing_uav_mass_ = getMass();
+    7109             :     }
+    7110             : 
+    7111           7 :     break;
+    7112             :   }
+    7113             : 
+    7114          10 :   ROS_INFO("[ControlManager]: switching emergency landing state %s -> %s", state_names[previous_state_landing_], state_names[current_state_landing_]);
+    7115          10 : }
+    7116             : 
+    7117             : //}
+    7118             : 
+    7119             : /* hover() //{ */
+    7120             : 
+    7121           0 : std::tuple<bool, std::string> ControlManager::hover(void) {
+    7122           0 :   if (!is_initialized_)
+    7123           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7124             : 
+    7125           0 :   if (eland_triggered_)
+    7126           0 :     return std::tuple(false, "cannot hover, eland already triggered");
+    7127             : 
+    7128           0 :   if (failsafe_triggered_)
+    7129           0 :     return std::tuple(false, "cannot hover, failsafe already triggered");
+    7130             : 
+    7131             :   {
+    7132           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7133             : 
+    7134           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7135           0 :     std_srvs::TriggerRequest            request;
+    7136             : 
+    7137           0 :     response = tracker_list_[active_tracker_idx_]->hover(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7138             : 
+    7139           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7140             : 
+    7141           0 :       return std::tuple(response->success, response->message);
+    7142             : 
+    7143             :     } else {
+    7144             : 
+    7145           0 :       std::stringstream ss;
+    7146           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'hover()' function!";
+    7147             : 
+    7148           0 :       return std::tuple(false, ss.str());
+    7149             :     }
+    7150             :   }
+    7151             : }
+    7152             : 
+    7153             : //}
+    7154             : 
+    7155             : /* //{ ehover() */
+    7156             : 
+    7157           2 : std::tuple<bool, std::string> ControlManager::ehover(void) {
+    7158             : 
+    7159           2 :   if (!is_initialized_)
+    7160           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7161             : 
+    7162           2 :   if (eland_triggered_)
+    7163           0 :     return std::tuple(false, "cannot ehover, eland already triggered");
+    7164             : 
+    7165           2 :   if (failsafe_triggered_)
+    7166           0 :     return std::tuple(false, "cannot ehover, failsafe already triggered");
+    7167             : 
+    7168             :   // copy the member variables
+    7169           4 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7170           2 :   auto active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7171             : 
+    7172           2 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7173             : 
+    7174           0 :     std::stringstream ss;
+    7175           0 :     ss << "can not trigger ehover while not flying";
+    7176           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7177             : 
+    7178           0 :     return std::tuple(false, ss.str());
+    7179             :   }
+    7180             : 
+    7181           2 :   ungripSrv();
+    7182             : 
+    7183             :   {
+    7184             : 
+    7185           2 :     auto [success, message] = switchTracker(_ehover_tracker_name_);
+    7186             : 
+    7187             :     // check if the tracker was successfully switched
+    7188             :     // this is vital, that is the core of the hover
+    7189           2 :     if (!success) {
+    7190             : 
+    7191           0 :       std::stringstream ss;
+    7192           0 :       ss << "error during switching to ehover tracker: '" << message << "'";
+    7193           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7194             : 
+    7195           0 :       return std::tuple(success, ss.str());
+    7196             :     }
+    7197             :   }
+    7198             : 
+    7199             :   {
+    7200           4 :     auto [success, message] = switchController(_eland_controller_name_);
+    7201             : 
+    7202             :     // check if the controller was successfully switched
+    7203             :     // this is not vital, we can continue without that
+    7204           2 :     if (!success) {
+    7205             : 
+    7206           0 :       std::stringstream ss;
+    7207           0 :       ss << "error during switching to ehover controller: '" << message << "'";
+    7208           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7209             :     }
+    7210             :   }
+    7211             : 
+    7212           2 :   std::stringstream ss;
+    7213           2 :   ss << "ehover activated";
+    7214           2 :   ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7215             : 
+    7216           2 :   callbacks_enabled_ = false;
+    7217             : 
+    7218           2 :   return std::tuple(true, ss.str());
+    7219             : }
+    7220             : 
+    7221             : //}
+    7222             : 
+    7223             : /* eland() //{ */
+    7224             : 
+    7225           7 : std::tuple<bool, std::string> ControlManager::eland(void) {
+    7226             : 
+    7227           7 :   if (!is_initialized_)
+    7228           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7229             : 
+    7230           7 :   if (eland_triggered_)
+    7231           0 :     return std::tuple(false, "cannot eland, eland already triggered");
+    7232             : 
+    7233           7 :   if (failsafe_triggered_)
+    7234           0 :     return std::tuple(false, "cannot eland, failsafe already triggered");
+    7235             : 
+    7236             :   // copy member variables
+    7237          14 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7238           7 :   auto active_tracker_idx  = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7239             : 
+    7240           7 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7241             : 
+    7242           0 :     std::stringstream ss;
+    7243           0 :     ss << "can not trigger eland while not flying";
+    7244           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7245             : 
+    7246           0 :     return std::tuple(false, ss.str());
+    7247             :   }
+    7248             : 
+    7249           7 :   if (_rc_emergency_handoff_) {
+    7250             : 
+    7251           0 :     toggleOutput(false);
+    7252             : 
+    7253           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7254             :   }
+    7255             : 
+    7256             :   {
+    7257           7 :     auto [success, message] = switchTracker(_ehover_tracker_name_);
+    7258             : 
+    7259             :     // check if the tracker was successfully switched
+    7260             :     // this is vital
+    7261           7 :     if (!success) {
+    7262             : 
+    7263           0 :       std::stringstream ss;
+    7264           0 :       ss << "error during switching to eland tracker: '" << message << "'";
+    7265           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7266             : 
+    7267           0 :       return std::tuple(success, ss.str());
+    7268             :     }
+    7269             :   }
+    7270             : 
+    7271             :   {
+    7272          14 :     auto [success, message] = switchController(_eland_controller_name_);
+    7273             : 
+    7274             :     // check if the controller was successfully switched
+    7275             :     // this is not vital, we can continue without it
+    7276           7 :     if (!success) {
+    7277             : 
+    7278           0 :       std::stringstream ss;
+    7279           0 :       ss << "error during switching to eland controller: '" << message << "'";
+    7280           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7281             :     }
+    7282             :   }
+    7283             : 
+    7284             :   // | ----------------- call the eland service ----------------- |
+    7285             : 
+    7286           7 :   std::stringstream ss;
+    7287             :   bool              success;
+    7288             : 
+    7289           7 :   if (elandSrv()) {
+    7290             : 
+    7291           7 :     changeLandingState(LANDING_STATE);
+    7292             : 
+    7293           7 :     odometryCallbacksSrv(false);
+    7294             : 
+    7295           7 :     ss << "eland activated";
+    7296           7 :     ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7297             : 
+    7298           7 :     success = true;
+    7299             : 
+    7300           7 :     callbacks_enabled_ = false;
+    7301             : 
+    7302             :   } else {
+    7303             : 
+    7304           0 :     ss << "error during activation of eland";
+    7305           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7306             : 
+    7307           0 :     success = false;
+    7308             :   }
+    7309             : 
+    7310          14 :   return std::tuple(success, ss.str());
+    7311             : }
+    7312             : 
+    7313             : //}
+    7314             : 
+    7315             : /* failsafe() //{ */
+    7316             : 
+    7317           8 : std::tuple<bool, std::string> ControlManager::failsafe(void) {
+    7318             : 
+    7319             :   // copy member variables
+    7320          16 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    7321           8 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7322           8 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7323             : 
+    7324           8 :   if (!is_initialized_) {
+    7325           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7326             :   }
+    7327             : 
+    7328           8 :   if (failsafe_triggered_) {
+    7329           0 :     return std::tuple(false, "cannot, failsafe already triggered");
+    7330             :   }
+    7331             : 
+    7332           8 :   if (active_tracker_idx == _null_tracker_idx_) {
+    7333             : 
+    7334           0 :     std::stringstream ss;
+    7335           0 :     ss << "can not trigger failsafe while not flying";
+    7336           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7337           0 :     return std::tuple(false, ss.str());
+    7338             :   }
+    7339             : 
+    7340           8 :   if (_rc_emergency_handoff_) {
+    7341             : 
+    7342           0 :     toggleOutput(false);
+    7343             : 
+    7344           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7345             :   }
+    7346             : 
+    7347           8 :   if (getLowestOuput(_hw_api_inputs_) == POSITION) {
+    7348           0 :     return eland();
+    7349             :   }
+    7350             : 
+    7351           8 :   if (_parachute_enabled_) {
+    7352             : 
+    7353           0 :     auto [success, message] = deployParachute();
+    7354             : 
+    7355           0 :     if (success) {
+    7356             : 
+    7357           0 :       std::stringstream ss;
+    7358           0 :       ss << "failsafe activated (parachute): '" << message << "'";
+    7359           0 :       ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    7360             : 
+    7361           0 :       return std::tuple(true, ss.str());
+    7362             : 
+    7363             :     } else {
+    7364             : 
+    7365           0 :       std::stringstream ss;
+    7366           0 :       ss << "could not deploy parachute: '" << message << "', continuing with normal failsafe";
+    7367           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7368             :     }
+    7369             :   }
+    7370             : 
+    7371           8 :   if (_failsafe_controller_idx_ != active_controller_idx) {
+    7372             : 
+    7373             :     try {
+    7374             : 
+    7375          16 :       std::scoped_lock lock(mutex_controller_list_);
+    7376             : 
+    7377           8 :       ROS_INFO("[ControlManager]: activating the controller '%s'", _failsafe_controller_name_.c_str());
+    7378           8 :       controller_list_[_failsafe_controller_idx_]->activate(last_control_output);
+    7379             : 
+    7380             :       {
+    7381          16 :         std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    7382             : 
+    7383             :         // update the time (used in failsafe)
+    7384           8 :         controller_tracker_switch_time_ = ros::Time::now();
+    7385             :       }
+    7386             : 
+    7387           8 :       failsafe_triggered_ = true;
+    7388           8 :       ROS_DEBUG("[ControlManager]: stopping eland timer");
+    7389           8 :       timer_eland_.stop();
+    7390           8 :       ROS_DEBUG("[ControlManager]: eland timer stopped");
+    7391             : 
+    7392           8 :       landing_uav_mass_ = getMass();
+    7393             : 
+    7394           8 :       eland_triggered_ = false;
+    7395           8 :       ROS_DEBUG("[ControlManager]: starting failsafe timer");
+    7396           8 :       timer_failsafe_.start();
+    7397           8 :       ROS_DEBUG("[ControlManager]: failsafe timer started");
+    7398             : 
+    7399           8 :       bumper_enabled_ = false;
+    7400             : 
+    7401           8 :       odometryCallbacksSrv(false);
+    7402             : 
+    7403           8 :       callbacks_enabled_ = false;
+    7404             : 
+    7405           8 :       ROS_INFO_THROTTLE(1.0, "[ControlManager]: the controller '%s' was activated", _failsafe_controller_name_.c_str());
+    7406             : 
+    7407             :       // super important, switch the active controller idx
+    7408             :       try {
+    7409           8 :         controller_list_[active_controller_idx_]->deactivate();
+    7410           8 :         active_controller_idx_ = _failsafe_controller_idx_;
+    7411             :       }
+    7412           0 :       catch (std::runtime_error& exrun) {
+    7413           0 :         ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not deactivate the controller '%s'", _controller_names_[active_controller_idx_].c_str());
+    7414             :       }
+    7415             :     }
+    7416           0 :     catch (std::runtime_error& exrun) {
+    7417           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: error during activation of the controller '%s'", _failsafe_controller_name_.c_str());
+    7418           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    7419             :     }
+    7420             :   }
+    7421             : 
+    7422          16 :   return std::tuple(true, "failsafe activated");
+    7423             : }
+    7424             : 
+    7425             : //}
+    7426             : 
+    7427             : /* escalatingFailsafe() //{ */
+    7428             : 
+    7429         145 : std::tuple<bool, std::string> ControlManager::escalatingFailsafe(void) {
+    7430         290 :   std::stringstream ss;
+    7431             : 
+    7432         145 :   if ((ros::Time::now() - escalating_failsafe_time_).toSec() < _escalating_failsafe_timeout_) {
+    7433             : 
+    7434         137 :     ss << "too soon for escalating failsafe";
+    7435         137 :     ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7436             : 
+    7437         137 :     return std::tuple(false, ss.str());
+    7438             :   }
+    7439             : 
+    7440           8 :   if (!output_enabled_) {
+    7441             : 
+    7442           0 :     ss << "not escalating failsafe, output is disabled";
+    7443           0 :     ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7444             : 
+    7445           0 :     return std::tuple(false, ss.str());
+    7446             :   }
+    7447             : 
+    7448           8 :   ROS_WARN("[ControlManager]: escalating failsafe triggered");
+    7449             : 
+    7450           8 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7451           8 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    7452             : 
+    7453          16 :   std::string active_tracker_name    = _tracker_names_[active_tracker_idx];
+    7454          16 :   std::string active_controller_name = _controller_names_[active_controller_idx];
+    7455             : 
+    7456           8 :   EscalatingFailsafeStates_t next_state = getNextEscFailsafeState();
+    7457             : 
+    7458           8 :   escalating_failsafe_time_ = ros::Time::now();
+    7459             : 
+    7460           8 :   switch (next_state) {
+    7461             : 
+    7462           0 :     case ESC_NONE_STATE: {
+    7463             : 
+    7464           0 :       ss << "escalating failsafe has run to impossible situation";
+    7465           0 :       ROS_ERROR_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7466             : 
+    7467           0 :       return std::tuple(false, "escalating failsafe has run to impossible situation");
+    7468             : 
+    7469             :       break;
+    7470             :     }
+    7471             : 
+    7472           2 :     case ESC_EHOVER_STATE: {
+    7473             : 
+    7474           2 :       ss << "escalating failsafe escalates to ehover";
+    7475           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7476             : 
+    7477           4 :       auto [success, message] = ehover();
+    7478             : 
+    7479           2 :       if (success) {
+    7480           2 :         state_escalating_failsafe_ = ESC_EHOVER_STATE;
+    7481             :       }
+    7482             : 
+    7483           2 :       return {success, message};
+    7484             : 
+    7485             :       break;
+    7486             :     }
+    7487             : 
+    7488           2 :     case ESC_ELAND_STATE: {
+    7489             : 
+    7490           2 :       ss << "escalating failsafe escalates to eland";
+    7491           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7492             : 
+    7493           4 :       auto [success, message] = eland();
+    7494             : 
+    7495           2 :       if (success) {
+    7496           2 :         state_escalating_failsafe_ = ESC_ELAND_STATE;
+    7497             :       }
+    7498             : 
+    7499           2 :       return {success, message};
+    7500             : 
+    7501             :       break;
+    7502             :     }
+    7503             : 
+    7504           2 :     case ESC_FAILSAFE_STATE: {
+    7505             : 
+    7506           2 :       escalating_failsafe_time_ = ros::Time::now();
+    7507             : 
+    7508           2 :       ss << "escalating failsafe escalates to failsafe";
+    7509           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7510             : 
+    7511           4 :       auto [success, message] = failsafe();
+    7512             : 
+    7513           2 :       if (success) {
+    7514           2 :         state_escalating_failsafe_ = ESC_FINISHED_STATE;
+    7515             :       }
+    7516             : 
+    7517           2 :       return {success, message};
+    7518             : 
+    7519             :       break;
+    7520             :     }
+    7521             : 
+    7522           2 :     case ESC_FINISHED_STATE: {
+    7523             : 
+    7524           2 :       escalating_failsafe_time_ = ros::Time::now();
+    7525             : 
+    7526           2 :       ss << "escalating failsafe has nothing more to do";
+    7527           2 :       ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+    7528             : 
+    7529           4 :       return std::tuple(false, "escalating failsafe has nothing more to do");
+    7530             : 
+    7531             :       break;
+    7532             :     }
+    7533             : 
+    7534           0 :     default: {
+    7535             : 
+    7536           0 :       break;
+    7537             :     }
+    7538             :   }
+    7539             : 
+    7540           0 :   ROS_ERROR("[ControlManager]: escalatingFailsafe() reached the final return, this should not happen!");
+    7541             : 
+    7542           0 :   return std::tuple(false, "escalating failsafe exception");
+    7543             : }
+    7544             : 
+    7545             : //}
+    7546             : 
+    7547             : /* getNextEscFailsafeState() //{ */
+    7548             : 
+    7549           8 : EscalatingFailsafeStates_t ControlManager::getNextEscFailsafeState(void) {
+    7550           8 :   EscalatingFailsafeStates_t current_state = state_escalating_failsafe_;
+    7551             : 
+    7552           8 :   switch (current_state) {
+    7553             : 
+    7554           2 :     case ESC_FINISHED_STATE: {
+    7555             : 
+    7556           2 :       return ESC_FINISHED_STATE;
+    7557             : 
+    7558             :       break;
+    7559             :     }
+    7560             : 
+    7561           2 :     case ESC_NONE_STATE: {
+    7562             : 
+    7563           2 :       if (_escalating_failsafe_ehover_) {
+    7564           2 :         return ESC_EHOVER_STATE;
+    7565           0 :       } else if (_escalating_failsafe_eland_) {
+    7566           0 :         return ESC_ELAND_STATE;
+    7567           0 :       } else if (_escalating_failsafe_failsafe_) {
+    7568           0 :         return ESC_FAILSAFE_STATE;
+    7569             :       } else {
+    7570           0 :         return ESC_FINISHED_STATE;
+    7571             :       }
+    7572             : 
+    7573             :       break;
+    7574             :     }
+    7575             : 
+    7576           2 :     case ESC_EHOVER_STATE: {
+    7577             : 
+    7578           2 :       if (_escalating_failsafe_eland_) {
+    7579           2 :         return ESC_ELAND_STATE;
+    7580           0 :       } else if (_escalating_failsafe_failsafe_) {
+    7581           0 :         return ESC_FAILSAFE_STATE;
+    7582             :       } else {
+    7583           0 :         return ESC_FINISHED_STATE;
+    7584             :       }
+    7585             : 
+    7586             :       break;
+    7587             :     }
+    7588             : 
+    7589           2 :     case ESC_ELAND_STATE: {
+    7590             : 
+    7591           2 :       if (_escalating_failsafe_failsafe_) {
+    7592           2 :         return ESC_FAILSAFE_STATE;
+    7593             :       } else {
+    7594           0 :         return ESC_FINISHED_STATE;
+    7595             :       }
+    7596             : 
+    7597             :       break;
+    7598             :     }
+    7599             : 
+    7600           0 :     case ESC_FAILSAFE_STATE: {
+    7601             : 
+    7602           0 :       return ESC_FINISHED_STATE;
+    7603             : 
+    7604             :       break;
+    7605             :     }
+    7606             :   }
+    7607             : 
+    7608           0 :   ROS_ERROR("[ControlManager]: getNextEscFailsafeState() reached the final return, this should not happen!");
+    7609             : 
+    7610           0 :   return ESC_NONE_STATE;
+    7611             : }
+    7612             : 
+    7613             : //}
+    7614             : 
+    7615             : // | ------------------- trajectory tracking ------------------ |
+    7616             : 
+    7617             : /* startTrajectoryTracking() //{ */
+    7618             : 
+    7619           1 : std::tuple<bool, std::string> ControlManager::startTrajectoryTracking(void) {
+    7620           1 :   if (!is_initialized_)
+    7621           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7622             : 
+    7623             :   {
+    7624           2 :     std::scoped_lock lock(mutex_tracker_list_);
+    7625             : 
+    7626           1 :     std_srvs::TriggerResponse::ConstPtr response;
+    7627           1 :     std_srvs::TriggerRequest            request;
+    7628             : 
+    7629             :     response =
+    7630           1 :         tracker_list_[active_tracker_idx_]->startTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7631             : 
+    7632           1 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7633             : 
+    7634           2 :       return std::tuple(response->success, response->message);
+    7635             : 
+    7636             :     } else {
+    7637             : 
+    7638           0 :       std::stringstream ss;
+    7639           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'startTrajectoryTracking()' function!";
+    7640             : 
+    7641           0 :       return std::tuple(false, ss.str());
+    7642             :     }
+    7643             :   }
+    7644             : }
+    7645             : 
+    7646             : //}
+    7647             : 
+    7648             : /* stopTrajectoryTracking() //{ */
+    7649             : 
+    7650           0 : std::tuple<bool, std::string> ControlManager::stopTrajectoryTracking(void) {
+    7651           0 :   if (!is_initialized_)
+    7652           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7653             : 
+    7654             :   {
+    7655           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7656             : 
+    7657           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7658           0 :     std_srvs::TriggerRequest            request;
+    7659             : 
+    7660             :     response =
+    7661           0 :         tracker_list_[active_tracker_idx_]->stopTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7662             : 
+    7663           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7664             : 
+    7665           0 :       return std::tuple(response->success, response->message);
+    7666             : 
+    7667             :     } else {
+    7668             : 
+    7669           0 :       std::stringstream ss;
+    7670           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'stopTrajectoryTracking()' function!";
+    7671             : 
+    7672           0 :       return std::tuple(false, ss.str());
+    7673             :     }
+    7674             :   }
+    7675             : }
+    7676             : 
+    7677             : //}
+    7678             : 
+    7679             : /* resumeTrajectoryTracking() //{ */
+    7680             : 
+    7681           0 : std::tuple<bool, std::string> ControlManager::resumeTrajectoryTracking(void) {
+    7682           0 :   if (!is_initialized_)
+    7683           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7684             : 
+    7685             :   {
+    7686           0 :     std::scoped_lock lock(mutex_tracker_list_);
+    7687             : 
+    7688           0 :     std_srvs::TriggerResponse::ConstPtr response;
+    7689           0 :     std_srvs::TriggerRequest            request;
+    7690             : 
+    7691             :     response =
+    7692           0 :         tracker_list_[active_tracker_idx_]->resumeTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7693             : 
+    7694           0 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7695             : 
+    7696           0 :       return std::tuple(response->success, response->message);
+    7697             : 
+    7698             :     } else {
+    7699             : 
+    7700           0 :       std::stringstream ss;
+    7701           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'resumeTrajectoryTracking()' function!";
+    7702             : 
+    7703           0 :       return std::tuple(false, ss.str());
+    7704             :     }
+    7705             :   }
+    7706             : }
+    7707             : 
+    7708             : //}
+    7709             : 
+    7710             : /* gotoTrajectoryStart() //{ */
+    7711             : 
+    7712           1 : std::tuple<bool, std::string> ControlManager::gotoTrajectoryStart(void) {
+    7713           1 :   if (!is_initialized_)
+    7714           0 :     return std::tuple(false, "the ControlManager is not initialized");
+    7715             : 
+    7716             :   {
+    7717           2 :     std::scoped_lock lock(mutex_tracker_list_);
+    7718             : 
+    7719           1 :     std_srvs::TriggerResponse::ConstPtr response;
+    7720           1 :     std_srvs::TriggerRequest            request;
+    7721             : 
+    7722           1 :     response = tracker_list_[active_tracker_idx_]->gotoTrajectoryStart(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+    7723             : 
+    7724           1 :     if (response != std_srvs::TriggerResponse::Ptr()) {
+    7725             : 
+    7726           2 :       return std::tuple(response->success, response->message);
+    7727             : 
+    7728             :     } else {
+    7729             : 
+    7730           0 :       std::stringstream ss;
+    7731           0 :       ss << "the tracker '" << _tracker_names_[active_tracker_idx_] << "' does not implement the 'gotoTrajectoryStart()' function!";
+    7732             : 
+    7733           0 :       return std::tuple(false, ss.str());
+    7734             :     }
+    7735             :   }
+    7736             : }
+    7737             : 
+    7738             : //}
+    7739             : 
+    7740             : // | ----------------- service client wrappers ---------------- |
+    7741             : 
+    7742             : /* arming() //{ */
+    7743             : 
+    7744          16 : std::tuple<bool, std::string> ControlManager::arming(const bool input) {
+    7745          32 :   std::stringstream ss;
+    7746             : 
+    7747          16 :   if (input) {
+    7748             : 
+    7749           0 :     ss << "not allowed to arm using the ControlManager, maybe later when we don't do bugs";
+    7750           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7751           0 :     return std::tuple(false, ss.str());
+    7752             :   }
+    7753             : 
+    7754          16 :   if (!input && !isOffboard()) {
+    7755             : 
+    7756           0 :     ss << "can not disarm, not in OFFBOARD mode";
+    7757           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7758           0 :     return std::tuple(false, ss.str());
+    7759             :   }
+    7760             : 
+    7761          16 :   if (!input && _rc_emergency_handoff_) {
+    7762             : 
+    7763           0 :     toggleOutput(false);
+    7764             : 
+    7765           0 :     return std::tuple(true, "RC emergency handoff is ON, disabling output");
+    7766             :   }
+    7767             : 
+    7768          16 :   std_srvs::SetBool srv_out;
+    7769             : 
+    7770          16 :   srv_out.request.data = input ? 1 : 0;  // arm or disarm?
+    7771             : 
+    7772          16 :   ROS_INFO("[ControlManager]: calling for %s", input ? "arming" : "disarming");
+    7773             : 
+    7774          16 :   if (sch_arming_.call(srv_out)) {
+    7775             : 
+    7776          16 :     if (srv_out.response.success) {
+    7777             : 
+    7778          16 :       ss << "service call for " << (input ? "arming" : "disarming") << " was successful";
+    7779          16 :       ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7780             : 
+    7781          16 :       if (!input) {
+    7782             : 
+    7783          16 :         toggleOutput(false);
+    7784             : 
+    7785          16 :         ROS_DEBUG("[ControlManager]: stopping failsafe timer");
+    7786          16 :         timer_failsafe_.stop();
+    7787          16 :         ROS_DEBUG("[ControlManager]: failsafe timer stopped");
+    7788             : 
+    7789          16 :         ROS_DEBUG("[ControlManager]: stopping the eland timer");
+    7790          16 :         timer_eland_.stop();
+    7791          16 :         ROS_DEBUG("[ControlManager]: eland timer stopped");
+    7792             :       }
+    7793             : 
+    7794             :     } else {
+    7795           0 :       ss << "service call for " << (input ? "arming" : "disarming") << " failed";
+    7796           0 :       ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7797             :     }
+    7798             : 
+    7799             :   } else {
+    7800           0 :     ss << "calling for " << (input ? "arming" : "disarming") << " resulted in failure: '" << srv_out.response.message << "'";
+    7801           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+    7802             :   }
+    7803             : 
+    7804          32 :   return std::tuple(srv_out.response.success, ss.str());
+    7805             : }
+    7806             : 
+    7807             : //}
+    7808             : 
+    7809             : /* odometryCallbacksSrv() //{ */
+    7810             : 
+    7811          15 : void ControlManager::odometryCallbacksSrv(const bool input) {
+    7812          15 :   ROS_INFO("[ControlManager]: switching odometry callbacks to %s", input ? "ON" : "OFF");
+    7813             : 
+    7814          30 :   std_srvs::SetBool srv;
+    7815             : 
+    7816          15 :   srv.request.data = input;
+    7817             : 
+    7818          15 :   bool res = sch_set_odometry_callbacks_.call(srv);
+    7819             : 
+    7820          15 :   if (res) {
+    7821             : 
+    7822          14 :     if (!srv.response.success) {
+    7823           0 :       ROS_WARN("[ControlManager]: service call for toggle odometry callbacks returned: '%s'", srv.response.message.c_str());
+    7824             :     }
+    7825             : 
+    7826             :   } else {
+    7827           1 :     ROS_ERROR("[ControlManager]: service call for toggle odometry callbacks failed!");
+    7828             :   }
+    7829          15 : }
+    7830             : 
+    7831             : //}
+    7832             : 
+    7833             : /* elandSrv() //{ */
+    7834             : 
+    7835           7 : bool ControlManager::elandSrv(void) {
+    7836           7 :   ROS_INFO("[ControlManager]: calling for eland");
+    7837             : 
+    7838          14 :   std_srvs::Trigger srv;
+    7839             : 
+    7840           7 :   bool res = sch_eland_.call(srv);
+    7841             : 
+    7842           7 :   if (res) {
+    7843             : 
+    7844           7 :     if (!srv.response.success) {
+    7845           0 :       ROS_WARN("[ControlManager]: service call for eland returned: '%s'", srv.response.message.c_str());
+    7846             :     }
+    7847             : 
+    7848           7 :     return srv.response.success;
+    7849             : 
+    7850             :   } else {
+    7851             : 
+    7852           0 :     ROS_ERROR("[ControlManager]: service call for eland failed!");
+    7853             : 
+    7854           0 :     return false;
+    7855             :   }
+    7856             : }
+    7857             : 
+    7858             : //}
+    7859             : 
+    7860             : /* parachuteSrv() //{ */
+    7861             : 
+    7862           0 : bool ControlManager::parachuteSrv(void) {
+    7863           0 :   ROS_INFO("[ControlManager]: calling for parachute deployment");
+    7864             : 
+    7865           0 :   std_srvs::Trigger srv;
+    7866             : 
+    7867           0 :   bool res = sch_parachute_.call(srv);
+    7868             : 
+    7869           0 :   if (res) {
+    7870             : 
+    7871           0 :     if (!srv.response.success) {
+    7872           0 :       ROS_WARN("[ControlManager]: service call for parachute deployment returned: '%s'", srv.response.message.c_str());
+    7873             :     }
+    7874             : 
+    7875           0 :     return srv.response.success;
+    7876             : 
+    7877             :   } else {
+    7878             : 
+    7879           0 :     ROS_ERROR("[ControlManager]: service call for parachute deployment failed!");
+    7880             : 
+    7881           0 :     return false;
+    7882             :   }
+    7883             : }
+    7884             : 
+    7885             : //}
+    7886             : 
+    7887             : /* ungripSrv() //{ */
+    7888             : 
+    7889          93 : void ControlManager::ungripSrv(void) {
+    7890         186 :   std_srvs::Trigger srv;
+    7891             : 
+    7892          93 :   bool res = sch_ungrip_.call(srv);
+    7893             : 
+    7894          93 :   if (res) {
+    7895             : 
+    7896           0 :     if (!srv.response.success) {
+    7897           0 :       ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: service call for ungripping payload returned: '%s'", srv.response.message.c_str());
+    7898             :     }
+    7899             : 
+    7900             :   } else {
+    7901          93 :     ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: service call for ungripping payload failed!");
+    7902             :   }
+    7903          93 : }
+    7904             : 
+    7905             : //}
+    7906             : 
+    7907             : // | ------------------------ routines ------------------------ |
+    7908             : 
+    7909             : /* toggleOutput() //{ */
+    7910             : 
+    7911         102 : void ControlManager::toggleOutput(const bool& input) {
+    7912             : 
+    7913         102 :   if (input == output_enabled_) {
+    7914           5 :     ROS_WARN_THROTTLE(0.1, "[ControlManager]: output is already %s", input ? "ON" : "OFF");
+    7915           5 :     return;
+    7916             :   }
+    7917             : 
+    7918          97 :   ROS_INFO("[ControlManager]: switching output %s", input ? "ON" : "OFF");
+    7919             : 
+    7920          97 :   output_enabled_ = input;
+    7921             : 
+    7922             :   // if switching output off, switch to NullTracker
+    7923          97 :   if (!output_enabled_) {
+    7924             : 
+    7925          18 :     ROS_INFO("[ControlManager]: switching to 'NullTracker' after switching output off");
+    7926             : 
+    7927          18 :     switchTracker(_null_tracker_name_);
+    7928             : 
+    7929          18 :     ROS_INFO_STREAM("[ControlManager]: switching to the controller '" << _eland_controller_name_ << "' after switching output off");
+    7930             : 
+    7931          18 :     switchController(_eland_controller_name_);
+    7932             : 
+    7933             :     // | --------- deactivate all trackers and controllers -------- |
+    7934             : 
+    7935         126 :     for (int i = 0; i < int(tracker_list_.size()); i++) {
+    7936             : 
+    7937         108 :       std::map<std::string, TrackerParams>::iterator it;
+    7938         108 :       it = trackers_.find(_tracker_names_[i]);
+    7939             : 
+    7940             :       try {
+    7941         108 :         ROS_INFO("[ControlManager]: deactivating the tracker '%s'", it->second.address.c_str());
+    7942         108 :         tracker_list_[i]->deactivate();
+    7943             :       }
+    7944           0 :       catch (std::runtime_error& ex) {
+    7945           0 :         ROS_ERROR("[ControlManager]: exception caught during tracker deactivation: '%s'", ex.what());
+    7946             :       }
+    7947             :     }
+    7948             : 
+    7949         108 :     for (int i = 0; i < int(controller_list_.size()); i++) {
+    7950             : 
+    7951          90 :       std::map<std::string, ControllerParams>::iterator it;
+    7952          90 :       it = controllers_.find(_controller_names_[i]);
+    7953             : 
+    7954             :       try {
+    7955          90 :         ROS_INFO("[ControlManager]: deactivating the controller '%s'", it->second.address.c_str());
+    7956          90 :         controller_list_[i]->deactivate();
+    7957             :       }
+    7958           0 :       catch (std::runtime_error& ex) {
+    7959           0 :         ROS_ERROR("[ControlManager]: exception caught during controller deactivation: '%s'", ex.what());
+    7960             :       }
+    7961             :     }
+    7962             : 
+    7963          18 :     timer_failsafe_.stop();
+    7964          18 :     timer_eland_.stop();
+    7965          18 :     timer_pirouette_.stop();
+    7966             : 
+    7967          18 :     offboard_mode_was_true_ = false;
+    7968             :   }
+    7969             : }
+    7970             : 
+    7971             : //}
+    7972             : 
+    7973             : /* switchTracker() //{ */
+    7974             : 
+    7975         186 : std::tuple<bool, std::string> ControlManager::switchTracker(const std::string& tracker_name) {
+    7976             : 
+    7977         558 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("switchTracker");
+    7978         558 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::switchTracker", scope_timer_logger_, scope_timer_enabled_);
+    7979             : 
+    7980             :   // copy member variables
+    7981         372 :   auto last_tracker_cmd   = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    7982         186 :   auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    7983             : 
+    7984         372 :   std::stringstream ss;
+    7985             : 
+    7986         186 :   if (!got_uav_state_) {
+    7987             : 
+    7988           0 :     ss << "can not switch tracker, missing odometry!";
+    7989           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    7990           0 :     return std::tuple(false, ss.str());
+    7991             :   }
+    7992             : 
+    7993         186 :   if (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) {
+    7994             : 
+    7995           0 :     ss << "can not switch tracker, missing odometry innovation!";
+    7996           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    7997           0 :     return std::tuple(false, ss.str());
+    7998             :   }
+    7999             : 
+    8000         186 :   auto new_tracker_idx = idxInVector(tracker_name, _tracker_names_);
+    8001             : 
+    8002             :   // check if the tracker exists
+    8003         186 :   if (!new_tracker_idx) {
+    8004           1 :     ss << "the tracker '" << tracker_name << "' does not exist!";
+    8005           1 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8006           1 :     return std::tuple(false, ss.str());
+    8007             :   }
+    8008             : 
+    8009             :   // check if the tracker is already active
+    8010         185 :   if (new_tracker_idx.value() == active_tracker_idx) {
+    8011          10 :     ss << "not switching, the tracker '" << tracker_name << "' is already active!";
+    8012          10 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8013          10 :     return std::tuple(true, ss.str());
+    8014             :   }
+    8015             : 
+    8016             :   {
+    8017         175 :     std::scoped_lock lock(mutex_tracker_list_);
+    8018             : 
+    8019             :     try {
+    8020             : 
+    8021         175 :       ROS_INFO("[ControlManager]: activating the tracker '%s'", _tracker_names_[new_tracker_idx.value()].c_str());
+    8022             : 
+    8023         175 :       auto [success, message] = tracker_list_[new_tracker_idx.value()]->activate(last_tracker_cmd);
+    8024             : 
+    8025         175 :       if (!success) {
+    8026             : 
+    8027           0 :         ss << "the tracker '" << tracker_name << "' could not be activated: '" << message << "'";
+    8028           0 :         ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8029           0 :         return std::tuple(false, ss.str());
+    8030             : 
+    8031             :       } else {
+    8032             : 
+    8033         175 :         ss << "the tracker '" << tracker_name << "' was activated";
+    8034         175 :         ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8035             : 
+    8036             :         {
+    8037         350 :           std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8038             : 
+    8039             :           // update the time (used in failsafe)
+    8040         175 :           controller_tracker_switch_time_ = ros::Time::now();
+    8041             :         }
+    8042             : 
+    8043             :         // super important, switch the active tracker idx
+    8044             :         try {
+    8045             : 
+    8046         175 :           ROS_INFO("[ControlManager]: deactivating '%s'", _tracker_names_[active_tracker_idx_].c_str());
+    8047         175 :           tracker_list_[active_tracker_idx_]->deactivate();
+    8048             : 
+    8049             :           // if switching from null tracker, re-activate the already active the controller
+    8050         175 :           if (active_tracker_idx_ == _null_tracker_idx_) {
+    8051             : 
+    8052          75 :             ROS_INFO("[ControlManager]: reactivating '%s' due to switching from 'NullTracker'", _controller_names_[active_controller_idx_].c_str());
+    8053             :             {
+    8054         150 :               std::scoped_lock lock(mutex_controller_list_);
+    8055             : 
+    8056          75 :               initializeControlOutput();
+    8057             : 
+    8058         150 :               auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8059             : 
+    8060          75 :               controller_list_[active_controller_idx_]->activate(last_control_output);
+    8061             : 
+    8062             :               {
+    8063         150 :                 std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8064             : 
+    8065             :                 // update the time (used in failsafe)
+    8066          75 :                 controller_tracker_switch_time_ = ros::Time::now();
+    8067             :               }
+    8068             :             }
+    8069             : 
+    8070             :             // if switching to null tracker, deactivate the active controller
+    8071         100 :           } else if (new_tracker_idx == _null_tracker_idx_) {
+    8072             : 
+    8073          15 :             ROS_INFO("[ControlManager]: deactivating '%s' due to switching to 'NullTracker'", _controller_names_[active_controller_idx_].c_str());
+    8074             :             {
+    8075          30 :               std::scoped_lock lock(mutex_controller_list_);
+    8076             : 
+    8077          15 :               controller_list_[active_controller_idx_]->deactivate();
+    8078             :             }
+    8079             : 
+    8080             :             {
+    8081          15 :               std::scoped_lock lock(mutex_last_tracker_cmd_);
+    8082             : 
+    8083          15 :               last_tracker_cmd_ = {};
+    8084             :             }
+    8085             : 
+    8086          15 :             initializeControlOutput();
+    8087             :           }
+    8088             : 
+    8089         175 :           active_tracker_idx_ = new_tracker_idx.value();
+    8090             :         }
+    8091           0 :         catch (std::runtime_error& exrun) {
+    8092           0 :           ROS_ERROR("[ControlManager]: could not deactivate the tracker '%s'", _tracker_names_[active_tracker_idx_].c_str());
+    8093             :         }
+    8094             :       }
+    8095             :     }
+    8096           0 :     catch (std::runtime_error& exrun) {
+    8097           0 :       ROS_ERROR("[ControlManager]: error during activation of the tracker '%s'", tracker_name.c_str());
+    8098           0 :       ROS_ERROR("[ControlManager]: exception: '%s'", exrun.what());
+    8099             :     }
+    8100             :   }
+    8101             : 
+    8102         175 :   return std::tuple(true, ss.str());
+    8103             : }
+    8104             : 
+    8105             : //}
+    8106             : 
+    8107             : /* switchController() //{ */
+    8108             : 
+    8109         187 : std::tuple<bool, std::string> ControlManager::switchController(const std::string& controller_name) {
+    8110             : 
+    8111         561 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("switchController");
+    8112         561 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::switchController", scope_timer_logger_, scope_timer_enabled_);
+    8113             : 
+    8114             :   // copy member variables
+    8115         374 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8116         187 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    8117             : 
+    8118         374 :   std::stringstream ss;
+    8119             : 
+    8120         187 :   if (!got_uav_state_) {
+    8121             : 
+    8122           0 :     ss << "can not switch controller, missing odometry!";
+    8123           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8124           0 :     return std::tuple(false, ss.str());
+    8125             :   }
+    8126             : 
+    8127         187 :   if (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) {
+    8128             : 
+    8129           0 :     ss << "can not switch controller, missing odometry innovation!";
+    8130           0 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8131           0 :     return std::tuple(false, ss.str());
+    8132             :   }
+    8133             : 
+    8134         187 :   auto new_controller_idx = idxInVector(controller_name, _controller_names_);
+    8135             : 
+    8136             :   // check if the controller exists
+    8137         187 :   if (!new_controller_idx) {
+    8138           1 :     ss << "the controller '" << controller_name << "' does not exist!";
+    8139           1 :     ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8140           1 :     return std::tuple(false, ss.str());
+    8141             :   }
+    8142             : 
+    8143             :   // check if the controller is not active
+    8144         186 :   if (new_controller_idx.value() == active_controller_idx) {
+    8145             : 
+    8146          31 :     ss << "not switching, the controller '" << controller_name << "' is already active!";
+    8147          31 :     ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8148          31 :     return std::tuple(true, ss.str());
+    8149             :   }
+    8150             : 
+    8151             :   {
+    8152         155 :     std::scoped_lock lock(mutex_controller_list_);
+    8153             : 
+    8154             :     try {
+    8155             : 
+    8156         155 :       ROS_INFO("[ControlManager]: activating the controller '%s'", _controller_names_[new_controller_idx.value()].c_str());
+    8157         155 :       if (!controller_list_[new_controller_idx.value()]->activate(last_control_output)) {
+    8158             : 
+    8159           0 :         ss << "the controller '" << controller_name << "' was not activated";
+    8160           0 :         ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+    8161           0 :         return std::tuple(false, ss.str());
+    8162             : 
+    8163             :       } else {
+    8164             : 
+    8165         155 :         ss << "the controller '" << controller_name << "' was activated";
+    8166         155 :         ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+    8167             : 
+    8168         155 :         ROS_INFO("[ControlManager]: triggering hover after switching to '%s', re-activating '%s'", _controller_names_[new_controller_idx.value()].c_str(),
+    8169             :                  _tracker_names_[active_tracker_idx_].c_str());
+    8170             : 
+    8171             :         // reactivate the current tracker
+    8172             :         // TODO this is not the most elegant way to restart the tracker after a controller switch
+    8173             :         // but it serves the purpose
+    8174             :         {
+    8175         155 :           std::scoped_lock lock(mutex_tracker_list_);
+    8176             : 
+    8177         155 :           tracker_list_[active_tracker_idx_]->deactivate();
+    8178         155 :           tracker_list_[active_tracker_idx_]->activate({});
+    8179             :         }
+    8180             : 
+    8181             :         {
+    8182         310 :           std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+    8183             : 
+    8184             :           // update the time (used in failsafe)
+    8185         155 :           controller_tracker_switch_time_ = ros::Time::now();
+    8186             :         }
+    8187             : 
+    8188             :         // super important, switch the active controller idx
+    8189             :         try {
+    8190             : 
+    8191         155 :           controller_list_[active_controller_idx_]->deactivate();
+    8192         155 :           active_controller_idx_ = new_controller_idx.value();
+    8193             :         }
+    8194           0 :         catch (std::runtime_error& exrun) {
+    8195           0 :           ROS_ERROR("[ControlManager]: could not deactivate controller '%s'", _controller_names_[active_controller_idx_].c_str());
+    8196             :         }
+    8197             :       }
+    8198             :     }
+    8199           0 :     catch (std::runtime_error& exrun) {
+    8200           0 :       ROS_ERROR("[ControlManager]: error during activation of controller '%s'", controller_name.c_str());
+    8201           0 :       ROS_ERROR("[ControlManager]: exception: '%s'", exrun.what());
+    8202             :     }
+    8203             :   }
+    8204             : 
+    8205         155 :   mrs_msgs::DynamicsConstraintsSrvRequest sanitized_constraints;
+    8206             : 
+    8207             :   {
+    8208         155 :     std::scoped_lock lock(mutex_constraints_);
+    8209             : 
+    8210         155 :     sanitized_constraints_ = current_constraints_;
+    8211         155 :     sanitized_constraints  = sanitized_constraints_;
+    8212             :   }
+    8213             : 
+    8214         155 :   setConstraintsToControllers(sanitized_constraints);
+    8215             : 
+    8216         155 :   return std::tuple(true, ss.str());
+    8217             : }
+    8218             : 
+    8219             : //}
+    8220             : 
+    8221             : /* updateTrackers() //{ */
+    8222             : 
+    8223      111592 : void ControlManager::updateTrackers(void) {
+    8224             : 
+    8225      223184 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateTrackers");
+    8226      223184 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::updateTrackers", scope_timer_logger_, scope_timer_enabled_);
+    8227             : 
+    8228             :   // copy member variables
+    8229      111592 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8230      111592 :   auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8231             : 
+    8232             :   // --------------------------------------------------------------
+    8233             :   // |                     Update the trackers                    |
+    8234             :   // --------------------------------------------------------------
+    8235             : 
+    8236           0 :   std::optional<mrs_msgs::TrackerCommand> tracker_command;
+    8237             : 
+    8238             :   unsigned int active_tracker_idx;
+    8239             : 
+    8240             :   {
+    8241      111592 :     std::scoped_lock lock(mutex_tracker_list_);
+    8242             : 
+    8243      111592 :     active_tracker_idx = active_tracker_idx_;
+    8244             : 
+    8245             :     // for each tracker
+    8246      782860 :     for (size_t i = 0; i < tracker_list_.size(); i++) {
+    8247             : 
+    8248      671268 :       if (i == active_tracker_idx) {
+    8249             : 
+    8250             :         try {
+    8251             :           // active tracker => update and retrieve the command
+    8252      111592 :           tracker_command = tracker_list_[i]->update(uav_state, last_control_output);
+    8253             :         }
+    8254           0 :         catch (std::runtime_error& exrun) {
+    8255           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: caught an exception while updating the active tracker (%s)", _tracker_names_[active_tracker_idx].c_str());
+    8256           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
+    8257           0 :           tracker_command = {};
+    8258             :         }
+    8259             : 
+    8260             :       } else {
+    8261             : 
+    8262             :         try {
+    8263             :           // nonactive tracker => just update without retrieving the command
+    8264      559676 :           tracker_list_[i]->update(uav_state, last_control_output);
+    8265             :         }
+    8266           0 :         catch (std::runtime_error& exrun) {
+    8267           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: caught an exception while updating the tracker '%s'", _tracker_names_[i].c_str());
+    8268           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
+    8269             :         }
+    8270             :       }
+    8271             :     }
+    8272             : 
+    8273      111592 :     if (active_tracker_idx == _null_tracker_idx_) {
+    8274       34708 :       return;
+    8275             :     }
+    8276             :   }
+    8277             : 
+    8278       76884 :   if (validateTrackerCommand(tracker_command, "ControlManager", "tracker_command")) {
+    8279             : 
+    8280      153768 :     std::scoped_lock lock(mutex_last_tracker_cmd_);
+    8281             : 
+    8282       76884 :     last_tracker_cmd_ = tracker_command;
+    8283             : 
+    8284             :     // | --------- fill in the potentially missing header --------- |
+    8285             : 
+    8286       76884 :     if (last_tracker_cmd_->header.frame_id == "") {
+    8287           0 :       last_tracker_cmd_->header.frame_id = uav_state.header.frame_id;
+    8288             :     }
+    8289             : 
+    8290       76884 :     if (last_tracker_cmd_->header.stamp == ros::Time(0)) {
+    8291           0 :       last_tracker_cmd_->header.stamp = ros::Time::now();
+    8292             :     }
+    8293             : 
+    8294             :   } else {
+    8295             : 
+    8296           0 :     if (active_tracker_idx == _ehover_tracker_idx_) {
+    8297             : 
+    8298           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the emergency tracker '%s' returned empty or invalid command!", _tracker_names_[active_tracker_idx].c_str());
+    8299           0 :       failsafe();
+    8300             : 
+    8301             :     } else {
+    8302             : 
+    8303           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the tracker '%s' returned empty or invalid command!", _tracker_names_[active_tracker_idx].c_str());
+    8304             : 
+    8305           0 :       if (_tracker_error_action_ == ELAND_STR) {
+    8306           0 :         eland();
+    8307           0 :       } else if (_tracker_error_action_ == EHOVER_STR) {
+    8308           0 :         ehover();
+    8309             :       } else {
+    8310           0 :         failsafe();
+    8311             :       }
+    8312             :     }
+    8313             :   }
+    8314             : }
+    8315             : 
+    8316             : //}
+    8317             : 
+    8318             : /* updateControllers() //{ */
+    8319             : 
+    8320      121312 : void ControlManager::updateControllers(const mrs_msgs::UavState& uav_state) {
+    8321             : 
+    8322      242624 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("updateControllers");
+    8323      242624 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::updateControllers", scope_timer_logger_, scope_timer_enabled_);
+    8324             : 
+    8325             :   // copy member variables
+    8326      121312 :   auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8327             : 
+    8328             :   // | ----------------- update the controllers ----------------- |
+    8329             : 
+    8330             :   // the trackers are not running
+    8331      121312 :   if (!last_tracker_cmd) {
+    8332             : 
+    8333             :     {
+    8334       69416 :       std::scoped_lock lock(mutex_controller_list_);
+    8335             : 
+    8336             :       // nonactive controller => just update without retrieving the command
+    8337      208248 :       for (int i = 0; i < int(controller_list_.size()); i++) {
+    8338      173540 :         controller_list_[i]->updateInactive(uav_state, last_tracker_cmd);
+    8339             :       }
+    8340             :     }
+    8341             : 
+    8342       34708 :     return;
+    8343             :   }
+    8344             : 
+    8345      173208 :   Controller::ControlOutput control_output;
+    8346             : 
+    8347             :   unsigned int active_controller_idx;
+    8348             : 
+    8349             :   {
+    8350      173208 :     std::scoped_lock lock(mutex_controller_list_);
+    8351             : 
+    8352       86604 :     active_controller_idx = active_controller_idx_;
+    8353             : 
+    8354             :     // for each controller
+    8355      519624 :     for (size_t i = 0; i < controller_list_.size(); i++) {
+    8356             : 
+    8357      433020 :       if (i == active_controller_idx) {
+    8358             : 
+    8359             :         try {
+    8360             :           // active controller => update and retrieve the command
+    8361       86604 :           control_output = controller_list_[active_controller_idx]->updateActive(uav_state, last_tracker_cmd.value());
+    8362             :         }
+    8363           0 :         catch (std::runtime_error& exrun) {
+    8364             : 
+    8365           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: an exception while updating the active controller (%s)",
+    8366             :                              _controller_names_[active_controller_idx].c_str());
+    8367           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
+    8368             :         }
+    8369             : 
+    8370             :       } else {
+    8371             : 
+    8372             :         try {
+    8373             :           // nonactive controller => just update without retrieving the command
+    8374      346416 :           controller_list_[i]->updateInactive(uav_state, last_tracker_cmd);
+    8375             :         }
+    8376           0 :         catch (std::runtime_error& exrun) {
+    8377             : 
+    8378           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the controller '%s'", _controller_names_[i].c_str());
+    8379           0 :           ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+    8380             :         }
+    8381             :       }
+    8382             :     }
+    8383             :   }
+    8384             : 
+    8385             :   // normally, the active controller returns a valid command
+    8386       86604 :   if (validateControlOutput(control_output, _hw_api_inputs_, "ControlManager", "control_output")) {
+    8387             : 
+    8388       86604 :     mrs_lib::set_mutexed(mutex_last_control_output_, control_output, last_control_output_);
+    8389             : 
+    8390             :     // but it can return an empty command, due to some critical internal error
+    8391             :     // which means we should trigger the failsafe landing
+    8392             :   } else {
+    8393             : 
+    8394             :     // only if the controller is still active, trigger escalating failsafe
+    8395             :     // if not active, we don't care, we should not ask the controller for
+    8396             :     // the result anyway -> this could mean a race condition occured
+    8397             :     // like it once happend during landing
+    8398           0 :     bool controller_status = false;
+    8399             : 
+    8400             :     {
+    8401           0 :       std::scoped_lock lock(mutex_controller_list_);
+    8402             : 
+    8403           0 :       controller_status = controller_list_[active_controller_idx]->getStatus().active;
+    8404             :     }
+    8405             : 
+    8406           0 :     if (controller_status) {
+    8407             : 
+    8408           0 :       if (failsafe_triggered_) {
+    8409             : 
+    8410           0 :         ROS_ERROR("[ControlManager]: disabling control, the active controller returned an empty command when failsafe was active");
+    8411             : 
+    8412           0 :         toggleOutput(false);
+    8413             : 
+    8414           0 :       } else if (eland_triggered_) {
+    8415             : 
+    8416           0 :         ROS_ERROR("[ControlManager]: triggering failsafe, the active controller returned an empty command when eland was active");
+    8417             : 
+    8418           0 :         failsafe();
+    8419             : 
+    8420             :       } else {
+    8421             : 
+    8422           0 :         ROS_ERROR("[ControlManager]: triggering eland, the active controller returned an empty command");
+    8423             : 
+    8424           0 :         eland();
+    8425             :       }
+    8426             :     }
+    8427             :   }
+    8428             : }
+    8429             : 
+    8430             : //}
+    8431             : 
+    8432             : /* publish() //{ */
+    8433             : 
+    8434      121312 : void ControlManager::publish(void) {
+    8435             : 
+    8436      242624 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("publish");
+    8437      242624 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("ControlManager::publish", scope_timer_logger_, scope_timer_enabled_);
+    8438             : 
+    8439             :   // copy member variables
+    8440      121312 :   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+    8441      121312 :   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8442      121312 :   auto active_tracker_idx    = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+    8443      121312 :   auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+    8444      121312 :   auto uav_state             = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8445             : 
+    8446      121312 :   publishControlReferenceOdom(last_tracker_cmd, last_control_output);
+    8447             : 
+    8448             :   // --------------------------------------------------------------
+    8449             :   // |                 Publish the control command                |
+    8450             :   // --------------------------------------------------------------
+    8451             : 
+    8452      121312 :   mrs_msgs::HwApiAttitudeCmd attitude_target;
+    8453      121312 :   attitude_target.stamp = ros::Time::now();
+    8454             : 
+    8455      121312 :   mrs_msgs::HwApiAttitudeRateCmd attitude_rate_target;
+    8456      121312 :   attitude_rate_target.stamp = ros::Time::now();
+    8457             : 
+    8458      121312 :   if (!output_enabled_) {
+    8459             : 
+    8460       20991 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: output is disabled");
+    8461             : 
+    8462      100321 :   } else if (active_tracker_idx == _null_tracker_idx_) {
+    8463             : 
+    8464       13719 :     ROS_WARN_THROTTLE(5.0, "[ControlManager]: 'NullTracker' is active, not controlling");
+    8465             : 
+    8466             :     Controller::HwApiOutputVariant output =
+    8467       27438 :         initializeDefaultOutput(_hw_api_inputs_, uav_state, _min_throttle_null_tracker_, common_handlers_->throttle_model.n_motors);
+    8468             : 
+    8469             :     {
+    8470       27438 :       std::scoped_lock lock(mutex_last_control_output_);
+    8471             : 
+    8472       13719 :       last_control_output_.control_output = output;
+    8473             :     }
+    8474             : 
+    8475       13719 :     control_output_publisher_.publish(output);
+    8476             : 
+    8477       86602 :   } else if (active_tracker_idx != _null_tracker_idx_ && !last_control_output.control_output) {
+    8478             : 
+    8479           0 :     ROS_WARN_THROTTLE(1.0, "[ControlManager]: the controller '%s' returned nil command, not publishing anything",
+    8480             :                       _controller_names_[active_controller_idx].c_str());
+    8481             : 
+    8482             :     Controller::HwApiOutputVariant output =
+    8483           0 :         initializeDefaultOutput(_hw_api_inputs_, uav_state, _min_throttle_null_tracker_, common_handlers_->throttle_model.n_motors);
+    8484             : 
+    8485           0 :     control_output_publisher_.publish(output);
+    8486             : 
+    8487       86602 :   } else if (last_control_output.control_output) {
+    8488             : 
+    8489       86602 :     if (validateHwApiAttitudeCmd(attitude_target, "ControlManager", "last_control_output.control_output")) {
+    8490       86602 :       control_output_publisher_.publish(last_control_output.control_output.value());
+    8491             :     } else {
+    8492           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the attitude cmd is not valid just before publishing!");
+    8493           0 :       return;
+    8494             :     }
+    8495             : 
+    8496             :   } else {
+    8497           0 :     ROS_ERROR_THROTTLE(1.0, "[ControlManager]: not publishing a control command");
+    8498             :   }
+    8499             : 
+    8500             :   // | ----------- publish the controller diagnostics ----------- |
+    8501             : 
+    8502      121312 :   ph_controller_diagnostics_.publish(last_control_output.diagnostics);
+    8503             : 
+    8504             :   // | --------- publish the applied throttle and thrust -------- |
+    8505             : 
+    8506      121312 :   auto throttle = extractThrottle(last_control_output);
+    8507             : 
+    8508      121312 :   if (throttle) {
+    8509             : 
+    8510             :     {
+    8511       95259 :       std_msgs::Float64 msg;
+    8512       95259 :       msg.data = throttle.value();
+    8513       95259 :       ph_throttle_.publish(msg);
+    8514             :     }
+    8515             : 
+    8516       95259 :     double thrust = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value());
+    8517             : 
+    8518             :     {
+    8519       95259 :       std_msgs::Float64 msg;
+    8520       95259 :       msg.data = thrust;
+    8521       95259 :       ph_thrust_.publish(msg);
+    8522             :     }
+    8523             :   }
+    8524             : 
+    8525             :   // | ----------------- publish tracker command ---------------- |
+    8526             : 
+    8527      121312 :   if (last_tracker_cmd) {
+    8528       86602 :     ph_tracker_cmd_.publish(last_tracker_cmd.value());
+    8529             :   }
+    8530             : 
+    8531             :   // | --------------- publish the odometry input --------------- |
+    8532             : 
+    8533      121312 :   if (last_control_output.control_output) {
+    8534             : 
+    8535      202418 :     mrs_msgs::EstimatorInput msg;
+    8536             : 
+    8537      101209 :     msg.header.frame_id = _uav_name_ + "/fcu";
+    8538      101209 :     msg.header.stamp    = ros::Time::now();
+    8539             : 
+    8540      101209 :     if (last_control_output.desired_unbiased_acceleration) {
+    8541       72756 :       msg.control_acceleration.x = last_control_output.desired_unbiased_acceleration.value()[0];
+    8542       72756 :       msg.control_acceleration.y = last_control_output.desired_unbiased_acceleration.value()[1];
+    8543       72756 :       msg.control_acceleration.z = last_control_output.desired_unbiased_acceleration.value()[2];
+    8544             :     }
+    8545             : 
+    8546      101209 :     if (last_control_output.desired_heading_rate) {
+    8547       69504 :       msg.control_hdg_rate = last_control_output.desired_heading_rate.value();
+    8548             :     }
+    8549             : 
+    8550      101209 :     if (last_control_output.desired_unbiased_acceleration) {
+    8551       72756 :       ph_mrs_odom_input_.publish(msg);
+    8552             :     }
+    8553             :   }
+    8554             : }
+    8555             : 
+    8556             : //}
+    8557             : 
+    8558             : /* deployParachute() //{ */
+    8559             : 
+    8560           0 : std::tuple<bool, std::string> ControlManager::deployParachute(void) {
+    8561             :   // if not enabled, return false
+    8562           0 :   if (!_parachute_enabled_) {
+    8563             : 
+    8564           0 :     std::stringstream ss;
+    8565           0 :     ss << "can not deploy parachute, it is disabled";
+    8566           0 :     return std::tuple(false, ss.str());
+    8567             :   }
+    8568             : 
+    8569             :   // we can not disarm if the drone is not in offboard mode
+    8570             :   // this is super important!
+    8571           0 :   if (!isOffboard()) {
+    8572             : 
+    8573           0 :     std::stringstream ss;
+    8574           0 :     ss << "can not deploy parachute, not in offboard mode";
+    8575           0 :     return std::tuple(false, ss.str());
+    8576             :   }
+    8577             : 
+    8578             :   // call the parachute service
+    8579           0 :   bool succ = parachuteSrv();
+    8580             : 
+    8581             :   // if the deployment was successful,
+    8582           0 :   if (succ) {
+    8583             : 
+    8584           0 :     arming(false);
+    8585             : 
+    8586           0 :     std::stringstream ss;
+    8587           0 :     ss << "parachute deployed";
+    8588             : 
+    8589           0 :     return std::tuple(true, ss.str());
+    8590             : 
+    8591             :   } else {
+    8592             : 
+    8593           0 :     std::stringstream ss;
+    8594           0 :     ss << "error during deployment of parachute";
+    8595             : 
+    8596           0 :     return std::tuple(false, ss.str());
+    8597             :   }
+    8598             : }
+    8599             : 
+    8600             : //}
+    8601             : 
+    8602             : /* velocityReferenceToReference() //{ */
+    8603             : 
+    8604         471 : mrs_msgs::ReferenceStamped ControlManager::velocityReferenceToReference(const mrs_msgs::VelocityReferenceStamped& vel_reference) {
+    8605             : 
+    8606         942 :   auto last_tracker_cmd    = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+    8607         942 :   auto uav_state           = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8608         471 :   auto current_constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+    8609             : 
+    8610         471 :   mrs_msgs::ReferenceStamped reference_out;
+    8611             : 
+    8612         471 :   reference_out.header = vel_reference.header;
+    8613             : 
+    8614         471 :   if (vel_reference.reference.use_heading) {
+    8615           0 :     reference_out.reference.heading = vel_reference.reference.heading;
+    8616         471 :   } else if (vel_reference.reference.use_heading_rate) {
+    8617         471 :     reference_out.reference.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading() + vel_reference.reference.use_heading_rate;
+    8618             :   } else {
+    8619           0 :     reference_out.reference.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    8620             :   }
+    8621             : 
+    8622         471 :   if (vel_reference.reference.use_altitude) {
+    8623           0 :     reference_out.reference.position.z = vel_reference.reference.altitude;
+    8624             :   } else {
+    8625             : 
+    8626         471 :     double stopping_time_z = 0;
+    8627             : 
+    8628         471 :     if (vel_reference.reference.velocity.x >= 0) {
+    8629         238 :       stopping_time_z = 1.5 * (fabs(vel_reference.reference.velocity.z) / current_constraints.constraints.vertical_ascending_acceleration) + 1.0;
+    8630             :     } else {
+    8631         233 :       stopping_time_z = 1.5 * (fabs(vel_reference.reference.velocity.z) / current_constraints.constraints.vertical_descending_acceleration) + 1.0;
+    8632             :     }
+    8633             : 
+    8634         471 :     reference_out.reference.position.z = last_tracker_cmd->position.z + vel_reference.reference.velocity.z * stopping_time_z;
+    8635             :   }
+    8636             : 
+    8637             :   {
+    8638         471 :     double stopping_time_x = 1.5 * (fabs(vel_reference.reference.velocity.x) / current_constraints.constraints.horizontal_acceleration) + 1.0;
+    8639         471 :     double stopping_time_y = 1.5 * (fabs(vel_reference.reference.velocity.y) / current_constraints.constraints.horizontal_acceleration) + 1.0;
+    8640             : 
+    8641         471 :     reference_out.reference.position.x = last_tracker_cmd->position.x + vel_reference.reference.velocity.x * stopping_time_x;
+    8642         471 :     reference_out.reference.position.y = last_tracker_cmd->position.y + vel_reference.reference.velocity.y * stopping_time_y;
+    8643             :   }
+    8644             : 
+    8645         942 :   return reference_out;
+    8646             : }
+    8647             : 
+    8648             : //}
+    8649             : 
+    8650             : /* publishControlReferenceOdom() //{ */
+    8651             : 
+    8652      121312 : void ControlManager::publishControlReferenceOdom([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& tracker_command,
+    8653             :                                                  [[maybe_unused]] const Controller::ControlOutput&               control_output) {
+    8654      121312 :   if (!tracker_command || !control_output.control_output) {
+    8655       34710 :     return;
+    8656             :   }
+    8657             : 
+    8658      173204 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    8659             : 
+    8660      173204 :   nav_msgs::Odometry msg;
+    8661             : 
+    8662       86602 :   msg.header = tracker_command->header;
+    8663             : 
+    8664       86602 :   if (tracker_command->use_position_horizontal) {
+    8665       86602 :     msg.pose.pose.position.x = tracker_command->position.x;
+    8666       86602 :     msg.pose.pose.position.y = tracker_command->position.y;
+    8667             :   } else {
+    8668           0 :     msg.pose.pose.position.x = uav_state.pose.position.x;
+    8669           0 :     msg.pose.pose.position.y = uav_state.pose.position.y;
+    8670             :   }
+    8671             : 
+    8672       86602 :   if (tracker_command->use_position_vertical) {
+    8673       86602 :     msg.pose.pose.position.z = tracker_command->position.z;
+    8674             :   } else {
+    8675           0 :     msg.pose.pose.position.z = uav_state.pose.position.z;
+    8676             :   }
+    8677             : 
+    8678             :   // transform the velocity in the reference to the child_frame
+    8679       86602 :   if (tracker_command->use_velocity_horizontal || tracker_command->use_velocity_vertical) {
+    8680             : 
+    8681       86602 :     msg.child_frame_id = _uav_name_ + "/" + _body_frame_;
+    8682             : 
+    8683      173204 :     geometry_msgs::Vector3Stamped velocity;
+    8684       86602 :     velocity.header = tracker_command->header;
+    8685             : 
+    8686       86602 :     if (tracker_command->use_velocity_horizontal) {
+    8687       86602 :       velocity.vector.x = tracker_command->velocity.x;
+    8688       86602 :       velocity.vector.y = tracker_command->velocity.y;
+    8689             :     }
+    8690             : 
+    8691       86602 :     if (tracker_command->use_velocity_vertical) {
+    8692       86602 :       velocity.vector.z = tracker_command->velocity.z;
+    8693             :     }
+    8694             : 
+    8695      173204 :     auto res = transformer_->transformSingle(velocity, msg.child_frame_id);
+    8696             : 
+    8697       86602 :     if (res) {
+    8698       86602 :       msg.twist.twist.linear.x = res.value().vector.x;
+    8699       86602 :       msg.twist.twist.linear.y = res.value().vector.y;
+    8700       86602 :       msg.twist.twist.linear.z = res.value().vector.z;
+    8701             :     } else {
+    8702           0 :       ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not transform the reference speed from '%s' to '%s'", velocity.header.frame_id.c_str(),
+    8703             :                          msg.child_frame_id.c_str());
+    8704             :     }
+    8705             :   }
+    8706             : 
+    8707             :   // fill in the orientation or heading
+    8708       86602 :   if (control_output.desired_orientation) {
+    8709       70726 :     msg.pose.pose.orientation = mrs_lib::AttitudeConverter(control_output.desired_orientation.value());
+    8710       15876 :   } else if (tracker_command->use_heading) {
+    8711       15876 :     msg.pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, tracker_command->heading);
+    8712             :   }
+    8713             : 
+    8714             :   // fill in the attitude rate
+    8715       86602 :   if (std::holds_alternative<mrs_msgs::HwApiAttitudeRateCmd>(control_output.control_output.value())) {
+    8716             : 
+    8717       65094 :     auto attitude_cmd = std::get<mrs_msgs::HwApiAttitudeRateCmd>(control_output.control_output.value());
+    8718             : 
+    8719       65094 :     msg.twist.twist.angular.x = attitude_cmd.body_rate.x;
+    8720       65094 :     msg.twist.twist.angular.y = attitude_cmd.body_rate.y;
+    8721       65094 :     msg.twist.twist.angular.z = attitude_cmd.body_rate.z;
+    8722             :   }
+    8723             : 
+    8724       86602 :   ph_control_reference_odom_.publish(msg);
+    8725             : }
+    8726             : 
+    8727             : //}
+    8728             : 
+    8729             : /* initializeControlOutput() //{ */
+    8730             : 
+    8731         172 : void ControlManager::initializeControlOutput(void) {
+    8732             : 
+    8733         172 :   Controller::ControlOutput controller_output;
+    8734             : 
+    8735         172 :   controller_output.diagnostics.total_mass       = _uav_mass_;
+    8736         172 :   controller_output.diagnostics.mass_difference  = 0.0;
+    8737         172 :   controller_output.diagnostics.disturbance_bx_b = _initial_body_disturbance_x_;
+    8738         172 :   controller_output.diagnostics.disturbance_by_b = _initial_body_disturbance_y_;
+    8739         172 :   controller_output.diagnostics.disturbance_wx_w = 0.0;
+    8740         172 :   controller_output.diagnostics.disturbance_wy_w = 0.0;
+    8741         172 :   controller_output.diagnostics.disturbance_bx_w = 0.0;
+    8742         172 :   controller_output.diagnostics.disturbance_by_w = 0.0;
+    8743         172 :   controller_output.diagnostics.controller       = "none";
+    8744             : 
+    8745         172 :   mrs_lib::set_mutexed(mutex_last_control_output_, controller_output, last_control_output_);
+    8746         172 : }
+    8747             : 
+    8748             : //}
+    8749             : 
+    8750             : }  // namespace control_manager
+    8751             : 
+    8752             : }  // namespace mrs_uav_managers
+    8753             : 
+    8754             : #include <pluginlib/class_list_macros.h>
+    8755          82 : 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..52a3d02708 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/control_manager.cpp.gcov.overview.html @@ -0,0 +1,2209 @@ + + + + + + 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 + + +
+ 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..50a187d9461b62335a1ac7991c04a23818ba9a1c GIT binary patch literal 25983 zcmV)eK&HQmP)oImN27<8*7l%>b^?=S<>EX~~zyb#;M%^k@kI_Pm z8qgq*YC!#R9X>{(_x7%3a+Ix^Jq8^-g$-? zN8rL3#|;x<95T>(e5}qjD`q2Aij1p$j1o?cn4va>jFIIdN09{BK29}82{0RH39xOP zEJmF%EHF|8Y-S{b(Ljt|hxce*4&w*Nqv3?au}1*xSlS4qg&4iv(>}OQ8soT)I$%8R zgFFtk0G89TEE?z4FVIr&c^2TQ4jTZMn*2wMHFGKgdf^e}DE;HHy!rr^GC2V7Sb_uW z86|LY1b3+19vsof`e{lHXzY3+C`G`LeNUJfDApyw;XR|Si`XZ0eRLA(*UcD=JH9&L zPP78!c&ZO-`BTV)t*J!J)ZFe7G~_V}Q@f4(F;l1p)No_oTn}?qe9p`~IzLwSxwsUR zfQuUAX&9ikLF;IccjgG}@F5qqV1p}LQU;7#QW1On-SY|kdBltF2#n0?>kj^F>7g_m#B+<|~x|KE}4@Yc~RZ#41Dwg%Q?(NpP2J zm+xtMOi{FJzkt^Cu3C!wHT+P*7y(DjC>mfb-b7_YVUpDt#~8V3Q}F)*ecGbhPqh|8 z6ybL)rW{bv>%r4m>UGI5mWWV=Q6j1XA2Ay{w#X_U!zNVaV_2yjER>>U5E3-}&P zr29KyjE>LrktaqMNVb1ZoZN7-8>rTXd?;@LEnWMi02z5qoak5z2`~GN3JY8fN*C8YWURDRaF z0-}(z_Zuhl*)|Xpw9*1ub4-`9VVn&|L`B!BBdxW!Lmn4kGaPr>JDzgD)8QEWbnZ8t zHedjkqa*g{Bu0U|3K`zjO+1dVq!0+VBhV6O#u^v><$eBE=oW^Mg?O6z$$p`u#du$6L*3$e#a0fRj` ztgIg_(vTR~>wiRkT&P3ai7B+Si##AZF*XB=GeZDNQ@EJ; z_&PxQ%q#_}A)swJaDKDS{TgB;BH#W!gN=7p&26_r`gS2yHlu*5s7GNggl@cKF>EuYzCz!#QZ}7;A!qyC#jeBWr+ai~wZ? z@D$-|fNkt?SCUPOt8c%K3p*ajp&z8l-xGb=$C6kwEab!eIIhCqG7pg(W5r5AgOe-( zma=vw6&)I3VxS{npbsGT*XS|AWmE{0!VB@{TNYP%IOs6zMYTW z>WR~FlucK}^+on*9*C?7E+is{og(Ilfv1U6@?Z~OPwssu#L%lS31i&?7XbUEhca9S zJkW*Teec@J#{w}&FuH??(Kbdm!eh905ssuSbA3GS;V}IH1E&!2Qy5}4M%AtTkRjqr zl9Jqj(Zy+00gA45ATbINQW1k&U{z()L?@FT%ig&l(q8HB&oaOiE*6;?hl|FTd!ziM zqevSFm`Gv)$x?7)#Cni&Q$1izW4Evet^XnE9O$M3-njT)>=@DdA2CQQh$tkG0m0$>B%NU;x-4^xhrJU^j;&M$j;R@Jcm*3rwA%< z37Z|bvWHWi>7M+jUO*1^D5yOFFnkXeY?0v=Km)^vYqMjvqX>XVFJx@$1zdM>{;33bf=A9WAVAtiOa+jhq`0A; z1gaTeftVap06b#QMP|=J<+;E|jCBAY8Rn?D-rEw?UJX!cNk#IOWIlkAAw;4vBIu?+FmH`=U#(m%8P-O~b%e+SK)|;8;yQvMMjrGyD#I9$C0w{i^%zGCGO#f6GL4q$ zIu5ol&|J&}J;edG5dnz-gd9bLkb7l}km1QE)-h5%#w8q1g$RxKq0nJAGtxL2hCe1H zpB!#xq*=66T|B|UnW=LgFogG4~Y$&Rt1u9=n{XHP;qVlY$Jjcy$o<q{;w19v!C#L^sf6y^pJ%#$NVR_74 zlQtjsjte;Hv8TPN2dN=9<(Q(W-M!rsf23XKs~R~%t+SeYUH#8=DrskNU<@Gs4Imul zu1|jO8O9|8TzuCCO34^?N)G{Q>+RLwV^WRrNTxdOi=Lf!j8fYV!(akLz&I8U*hEY? z=R&bo z3r&;sPy{1fcB$gATkL!|QnubY?)3u35@VWb=oL}SNB`(p7uDzdVZ;0dK_f=kN`P$`{Tpy1U>x`+_D;l%eKnEk8gfbm z+&o6j!*V)6k5Q6%@(yG^n_C1_HiS_SkG-`{;Yy`A{`^VnddQo*uI1~R*O48oaKpkN z0RH~F4p576({~+3EeC9)I!JELNl{6F;DrcePaKkN$%9hMBi~1;_--04@ASP>XCSr&ye!*c5J;h(jzbg=vZHJ8U!K;qGZJ6WcAdFyoN`PR-P4cLpqLVK8 zL)r`dAc66rW>3Pn$p_Nu7C;Gk12J2oh--!^JhUvd@)VLeMBXt>wFrpqM?HHy#?o2A z0qQ1OPxa5w{eHXCcD>zxaQg-PAu!?3Yxw!`zdHo{DYaif+KC9bk1-n)11OC#jxqxr zG)9A%p;_H_`PtGOC!u(zwTC+I(&6m+zV(FUSH(uZ*|tMI)o8M>x5xi)yF99<`gg8% zQ*D{+BEQpGS6L~W!0;U1Wym&+RJ3eAX22ytgj3h`CXW$e>lR$v*76w17&I7nly$&O zhcUTl^|qM~VYTas^10R?Iw-Yt{ky#a%VYo8=##?x^ok_$v57mc38SRQ&)3J!cKd84 zMc|r*;xQM%_#so%J~JEsIZjP=W^OmPjlAv5Acm{pn)D-{7`kNIX9iD9S~As{2@~^% zYbJM<6lMk;UDX?Vb>|wbnhm=`1b%wg&m1GsRPj3##-fsPqx}8qBa~Fib{P~BTb;td zERD&vX#l~RQ3H;VaMyHZp&!C9ssZ3gVm4*h7ws7J>~Vl&h*4Y3q2qB%3IYzBnN`*G zMSc1J2cP~iV)fVOeqhwI`Hp?0W09vCn7SqQ=r6iiK8k=Cblg&~sAk>yIt-v&%(5_+ zHw4TkTm~ffIwBywhU^E_k1+$(PGKxP*=#Bd=OYaz(HZPp3)O6W4TFI++%aTW*Y;fj zTz>Y>HJQTb>JfuEwPPG|5Z5tW>G~ucPq`%>X_4-u7~S=zG&I+dmXvlWfK@o4YvMQ` z#>Z&wX))4AH+*0Rw_K5fXLG&5t5R3=p3Md>X@2* zJjEb?7tvWDM#^ij8|;g^44Af7$|K8Y@(VAHhX;4 z&BqM!)^D3_w0Za=A9J z$Mc0f7-xX^sJ9g`p80LA4A@GHDk-+~}icm08(XLWDg@P#@FjXiDilSF2Zdglv%q7XbT6!mA`l@K{LTRm67XMKoHGc#i$3|)EHM4U`#qK1DZqA&%ty4Xzn{azz}hc03B$S(H)Wo z?0_5BwXYCDF=o68STn$li;`S71|tW!?aLJcZX7Xd$h1=v(=an2F9&K=HpwsV^wQ8Z zT$?389mXHQBy~6PH}{u!Ke&dhi5^p7M}~_4u-7DqtXo%VChLC()L1lKV`N7uj`2!H z`Ho|}y<&`f{ky_kGk{OvBg4zW7%?xOHby=gM;s&M<@5NMNn~kVBZ+*{7&(b7@-YMa zPuC2?4sML9v!=dlvKP`#HEqLjjJ2-aUx|mosTOK6hO_e&s^?tLgu7<{|L_*$ephSl zF&Z%8;Fev$SdEV~gVA`(s$*^&Ii_qbk3u$(z<^4C8xWYRSD)dzJ+JG$d!}o-?ll1v z8BWL8T}eAzg|03ki!q1eF{+|$WInDL;Fphuu4lL(U=X4zcuvm7@qjaf=N`hN3PR*r z{hTot*qOT@1a?je@bk=pGKs$N+U&x$o@B}XTmTdZ`0lv?S(IG_{JdNMH|#&33$R~3 zJpLxDP$}W!l?J(QG+$|e5vHF=K9_KVlVenN{mlh%kWGM%#C&rB#*Xou3(z;lSLFg2 z##l%i0I7o>u#{*U0iT{U;L*Hw(t!Ww<2QDq!?l7tAp8kjEwHolO1%xQefJ%&KW!Gmdpk<5zRxvXUybd_9Yb?$(AQ5{dF}!=9 zFCNGODjQM&AZh>Dt^gYL!Vq60j8RPtz*x+9>ACUYu};`sIn@mJj{zl)ACkjTji{7k zFNjLv4G#0b z)B_5uKhtJ@Ks-j9;D!#Esb29Iw~9=3`EjKPHV9J0sEO~}I^U^KOkrV=nlY{pqjrqs z!5_S`#+(ZUwZiQ)tOzyxAMqnTJ;df8S;&=FS0089T@QT(dT)&{-yE{{ z6{l9cg}?s$KQji}#9KYE21CZ8atbd|4vMM(9{1#Plt}?_%az;dup0-2Ywt+|AvX3B zqT&_OJICLXMwabaEisyOxdKYv2giU;hF=&JNuAP;o)r_q0G1LFCoo*xRqMoTmn=Xd zcg0KOyc;o0w8=XXGfd1LYDXC3GiL_R4o?g-JM4`QH#^TIhQ6jH5_2KiEMo~fJ5Qb& zlAOj7vuTLoSIskt*@{a6z0F0F{513ORRDEjF8P4iu9Z#jkurd4J6zqS*x}yFJr2Un z*tqd-^8%K3ligEfsXOi|PI8AjhzC@f*V_d1I=tn(Pfv;A!E`-~y{3!B^pCO+VpCwe zTwJL?l*Ob1+`kb|LD&u#Fgo|Fo{!OMlpP)^ABp8JV4MJ`C#LD#93EEzUrjtW8{>Pn zf=q0lJrR(V2@rU3@kMISqByDnFRm>33jvivA)l{fRQ3J=V$8rj=>GqA$pvuNLa)dL zDDN}snQ5W=6l3upe+PhPJZMo)F=~*n62VYeU zP>m6v0VK)DQ|N(`ECSL!WMcvMul5oG8{VBeh$+aUx(dR3Xxvk<6c$blKT-D}1FXr9 zOLy0yF~jAcSlD&Jl5AC^$e4OApqzo#prk6fv2eKpF0z0#ik8H_5&c=gBm>nYpJt$} zXLqI7_L~Hl-<9-sEhSaG$HpG?BZ@S7&SPVg?}U$LhL;vXruJ5Lt{<(}h>RQ7jGZQw?Xh2k&1lUc=Iv>5YC z_83*Lrkm^@jA;Mdw15%3sq3|LChgk`F|$ILtpRXQ*PcKVpswp(S1h}WFuthZ9x^^= zw|(io^v4HLQ-u1Mm5|duF%!xgS*SkkI&{B}Tf)6u&*gK#7_azz%sWQ;AZEWn+8#t? z3M1BMGO>r|h|txwF@Pa&Ib&mvku!`Z0_v$=d94dy9I#>B2PqdAY{iI2N5;EN1JsQ1 z(H^V4D&D)19aFVso80s~&n%NYltYS3n?RkvtFQ*Br~11Ji-4Lj{<&2+x16=$+wZ(N zquxaZP>!g|Fb2SC*B+3&cHsco+r0^Zs>bp$f91iFX-6?i=e^!r(j7ONah{kR2cB^O zqXVS3DIy@8@i7>i0h{^w%Q_yg8Dl~Wn%S~nl5IaApfq5tE2)A_5jVtkn?h(>z|~TL z1z?m*5w(~Kct|NDAOgBA?z(%Ce|NdvHO%c~K6>^F9~-#EzizkqS`PpO-$vIn-86;w zdr{YCM~w^JSp745GJBv$)_>k#w@Ca*fbiQe0TiRg1*sSBofb}9NH>!JHLW1wxoQ@a z_=;SN0!E?h&N1Tl08}}VtxD>lrs&PX|DvP@bZv`u{p!cBe&nWbIH0C|^jz5xW zqP~W*CwY*@V4N|=c~huzkmSySi#tQEd+xi>+J5|W_^hDVa5v-Kn@_#+Wh6008(5?# z!*c0WDHj)%PzN+&+y}Rnip4ikEr+jNn&K&b<+l|6fEZo@RCb*ToK=t3gXO zgfr^VFx8yD^=+uG3(q-)UY5r=1yIMHbG~(TVw}bBQ6lzeiH}<;Irh>5_&?1|8l$Y} z2{8i!byOpa1-tmgfU1Utq7aqWcT*t%j!xZ~q$rjc?o|T(8gXAEt~iBXBd)cb|B?}> zZs)JHH>%CB_O`N*;^JRKM26mw5UmTeA4?d=16s!T-a>Ky8k%22lL8vrXdNY77BIxT zRZ-RX_?^i8SXSWxvhs4;{Av0doeZ6^gfyUp;(EfkOrtvu-UI#nTf8{J%>k zx^XHhAv!)vfGen5xaWs!i!eU75+bO!Q%qh)dm;Et7CX2wg<>h$zW6y3;CM%qmKObE~@&QNZ0$?k`j1q|eTL}5^oB&SHPF+=1|KEo@T7YTO5C`ge9Qqn=z=PxelaE!ve0102(C7i73^b^TW)zwRC*{?%KT(zV#-q<-n z5C}xR=luYt%qGw9_(w?13lBmyy@#^+$_K%u3O_9%HX(#6#92OugxvUtYsgAr?4Go^ zUa6X2T7=Qdf)4Op2Ydte1j$B=GL5V8%vEqhHtbwf5Xsj~UIn_O5&b4WbJs;5(gcTw zz{hNj&*7sP<4f67a6t4quN=qmhgVus@j}BTOBC1>W`&<(FxT$yb1=-%0c}lYuk8RC zoKe6l<@Pc-aulq{ussNreZO{_<`Op681&b)UrcAjeE4Cxhj$8WGe`5>*+eLNkge#FQPLe z&5Sows54{5JbLYM4?buizi5#Fi*Z-RmrjdbZsa%*kp<}#b&PdyKCNCeWF6omvKDN~ zX_*sM&=dxYYYEl<?_CT+5s{Ub?GC8T5Yvk0 zXgGZ{-E+k!m_w1TxD`J=lY}X4*L{by(dq9ybFq*9o_*hps-=J%OJ#-;`^}=O?&kmV z#Zhl}gWpU&GltUTT0ZV-Z{38ws1c*z*lV|>H9At)7s*H=fVwQUA7A&DZ?~8+yL(;H zzo2RN1#oZ9J*%2*%UmZHJ14&D7T)MqHnbsS%*>N_)iYDyHCwkhdDpaVanLA8NY@Nd zjZxe+x43&kj0g_1eY0+wJ2K4}HH+)fUbSn!c6a4A*KnSk`#yL=d5oo}6T;Th*s5-7jLdy_KHk55Bo#K-jw2~R82;Gp z)h3K}z*n6#)k`54Gj>gU#jK7|VZ-sBj|*Yg9;0cP!n4MP`+J^`WcMs3yT=zUKcP0Y5b(#EY?Q|zrOnl7AQfybqk*Kbxoj9K2kcuoUxRja zV^2iwKkXRR4Rl=}r^jX{mu|JL87ewKWTF&Zt!svnDHc%YYBtED`KDrtVSY*K}e81vh~|Y~S2+)pouAnbDG? z%q^+C^k@7$rfcs3Ll<4Z9hRA8xPR4>&Lu56kdAy>yRy!pvQetx{od98|AweWzmeND zZj9pw=v%&ELtH#rx<0%kK!^y(vN3n2?wk?COd+=>*QdT=Hp*~=x#PNaDy$53rin;G z#4-Wg#23ORM5e-+uj20Ecdk;#P$Wh>MmK}W-Z&w>5tck`?AVAjUA8z_;3x$tBm!z7;xwxZYf(i&`i&B4IhtEXQz%+jfLt_&qO~d{|i? zE|4J1G4yO24nEq{Z+%jgzU!#_<$Ym~`GC~EF!fu`{BBCChBPvO`ds_rJ^)WOcLFBK zl2)o~fp!P^4iH=eUP9#lJ0z2@q1FaRjI!$qbrm+6!W~1^8>g`Coptv!S!2*MlrDro zgnC$)#D$a+LP4s_k@%XzmH>Zai5Eu%qpb6%d9V0%w|rrg0#4z!d@BID?fT}=?jEo% zXA?qLVO_r9#bK@7V!1ugFd)bbPFi9$7SX}!MbY0}pThh+5yzMvKUi&K4ZWno6Z?<> zV}=TYaqt@u=hD>pJNAdUVoIuNepQ0wPL=~1xx?gN&(3!RA-G78g7o;XzKa-E-8wTD z@XiMMgRrMWwTd_Alpz)_2g+kaCz>JIE)3aO#X$&I#W;1%(2#v%$p3&#(uwuWMin^AkEH1vSm3PfNLtJ!dF=^_ zCwav(Km0U&d56=mT(HdQ$JLc|vSZW$8Zf@p2f%pZ!WSX=>_2rD)Yua^^h)-8Pt8M4 z&8db)Wg#Bqvxhf+Hut?vKWdJJS-`1qENrQcmvBgcYK)yHBnIPz6OuHC@cd|*!r>RW zN;c9YXz`#q!%^8RxKbFuG0}#f!VMlZ8!%P@|7Jnrufg844ZqO5hE;(Df2J{``v8(8 zLhYv`r{AQzVf5o>NvkH|0wDx~nL=fw7;oXP7;oldXv6#0ciO0|=!F8?k9?p~7cu+* zML>?2DHdsn*bVOZW#J+egaqSEfZ8LaL)ctSbtA)lY+@9kIHS!N9}><^ff-|UBB#x@ zVzNJ0Q(R+=(hUH%VH`Hs8o)_&eXK5P{{A|j>gIJVt!i8BH8??nu)fThi|;<+?vU5L z)^*K^nJf8nLtV|9!bInB$;howVRIs4beupgW+YmtFoe$=m>{;?-y=?;a*TQ0ewf}A z`}k-~pd=63mKXQCS#kLSW@5k?ALxL*!Z|GgqCoZ{MhKd*=Z2RG2DB4%_p&*TJpp5H zmIj*q>x~c0?TN=Fds{2U>tjZ$fc7yyet67p6%YwR8Bl}KGvpU=ATja@00UmHvgV%( zc>AdWY8tQ2x)YY%n);oR!oFz4_*h=QB;Q;t`T&fHRr2u48 zPHyp4OXJLFZo2cCVII`mS}TVHnxw*|I*dQLHJsE?`tOERN^b1p<;r6WNsu>(-f{#O zeeZHc0x7JYOROJ01@KY`1GuymOv)>>wDf*Zq9gtxM8Mxn@lpTD@z03Tm+af3O=d#);; z26!XN=VoM&(k*n*pZ9?N!~@M3uAag?awhRa2IPyD_H-?`CX7O$NdO$at}|NXF>1Sh zolsD}P%L+s*T-#^9-cvOBt4>t7;4$+g}5RsF=wc3 zfc&-+b3O~#VOl_VN%LqOT?=>OR*Zl(0^^&<2rgbj{kDxMJHd9k|C;ThJ68Lo^9==g z1Tp?cUyGr03K_N1FOSgW-!+A>Wq>c%;R<4ARFjAgphR^R>%=5qe_q~bK+%$XjzitT zO$0PzjN5t{P>9m7>2p-}5JR$j z(H$4XsFI32xZ9rx`nCX!A%}o~6W*|SjjFG*!wvW-x^)VWsah^h;YR6I# z^YM*i#JcB3HoPkYRAXccRK(~bw*~{CF8%W7N2hZ;5{$#paB^DO;s{$Zd3#LGU{y+o zC|w3xF2sBdmiRs;@c2AF$X(w+hWVuYvkF&N|6BnvwM`hSuvY#Ql8V)t6ci0TI?!RU!hi%z6isgD26AHEDy zMt2T~DHkj$kLZ@e<$#Fk0C*0dZf1U5wKmpkwBz%C+b~{h@5=u0+c3K-<^YOSfJ$RT zo{Fn~yZ7-IW)Ed1gHch!bNu$9Z@9|8h@Vuv3Ma_?33Cl0o*3m05hq3z-PpC#!RGDE z@^SqqTZN)b7`j%slIa2aYS=!C0j!fl)05lr7(7`^2DqSe0?!_e$=t@C3X9q>W-A5N zR{Ud8pM!6F_{c+g7rR*^3*J6?cT!P##4*=(&l|Kayzx=qbGMh<+N5YdeH znBt7}#?W-q{2DwS)AM2mN)p4@Y_R(`t96WLQq*as0{*DUXp-^0F3dDoYaCxHUGF<>PlC&S2vb6a;V zMfAg2nL%f%G4zdosC51A(GQtW2#1;>Lh+4$c(qXIFrLpsF-s^Z*1b?sp>op#zP6a9 zzP&!5173!Q`?mcbCgyXz)R~$#w;-jGJX$dN17;DhVT|d|+-Z=nh`C$)Ysw00#t51k@ZOk*l*Muh(ljr!z&qBxWw?K1 z7B47fA(6DCe)xBZ^LK(xd?aN`ML;!1KF1G8SQo~d^bTy^b<(*R;P7D18(F3TC;OHw zWaBLXwqSH@l*Y)ikpY|;w&N9eT3`E@Z8|*Mv6Wmb{Nl=AZ*E^)QVeVN;tDCEO+o=N z?y>Zu;9~Jkl0*ULFRuQy!6F-pui1>zWpZe`4s7I}=$%{z0cLV_Ev{oQ9G75l<#@QF z^6B0xwf4m|B$t$viY#Bc9iz0on9L@Ac|B!u@to}5xw1#=n=y*ZD>F2uWG7E2N#Nh%AjTpa1+&RuZ3gErh!HcRT^$=uqI{@3uoe*52KN zxs9^M;&(1!-vhvx7i$ZL(%@8#`6Zd)u3PQtL|uyvFRp`g6tl{u>$xP4Ta@|$_%ZTW z4s^xrsh2J9P&+eeBd!6e;^!?9mHL)eKU+1{K=)PJ#cuvMx4)>ZW=~arFJ?aKVDDUd$nfBun4pJ$SJtxrz zyZeB5>6COnBlp$*w?qt`7p}sHiRu^nNXQ*Ktd_+3_8`>~dsI|2RyfJ4-eMB84rA#6 z8S1q^+^QdQfRu7^Ae)+1bN(Qj|7gXyhF^!vpizhK7!@vGtizMQWLuaskThK%UiS>t zXShD|ropSyE0Kj2I z@9wJA`g2gOR>N|gy1NdF(v){s{QZaQu2c3xkMDa{pdEa~rYXdG_>z_>6Vjwot^r$V zdaP#Qtzs0K04-hrCH*+ikMd{*Ukdt%kMpIBdB&qjKZZ=8Yq+>>dj0aNAB%wMCf}Po z?@EaMvR^EMd-L9S+!pUl8PL-8U-Hfjb~e|1&Ea1^Pd^qEWouDTw7Ps%{rwM13Y01; zrD2TKMPmU}c0E3A(7GXo9f(ukM(Hu^GwPko)^%>=lB3w9?c*=$$KyE9^`nXU5r>%22+Kugzu$&GUys(K2#r@7MCd+46hRT5mT ziZObDyp@e%ot(`pnHa)Hm+-7!4A?dc3MtDaGARK1AFo->FbWtjiu&2Aq`1q^zWW*7 z<);!Or~%Imv>l^aa3B7~E9%qS^|T|Y?OX_-hq?OOiV%S@X?OjT+sBxT(mxg-o}EM2 zczkOs9H4!%;mn8Z!2_|e>y+YKt>CA}m`avZ9<|hAY`IYCiN>?{iB#_)b*W=#kCaW-w6uzL`hH+2~yVCdh=bozb8L5MA3o3@@8t48) z4NWAkV2pZVBC=uD-h}ibAL-&BeZED{s;}1ILSQ~M(j~+e)7KCz7+)3X;&#SyTb!S3 zXzg5mDJjYxn-5?h!{=Cu@ zD(%?Bo}wE4pO@>{asaC3`e<#-wena*+-9U#Z^Cl57)UR=fwcQ^?vNH;|QUsU8FA z0_LC#<1mZ#5FT8I)!G-PA_04)$t-RRxP|-75;2MEtRxw)e8mcgEp-cb39mYun%s)_ z^v5+;1sL#@=TpN&-y_`(X}0i0hIwXGHNo34M!m$v0Wj+Ttf7Xkdu`l~Rj7IXfpoq1 zSbB6rQI7m9`1ZIq!P*P8)t7MG3xPCss>NpsRkRqZu!bQPBYY}vS?(<`Ts6uD>GAs# zw-=eEXD}bpTb10oYmI==97e387}bDV-MxPHhqJKJT(d{P@8K5fie1Q>cOVtM=9jvV z|I!g4`g*^qI~kxlS}`VwFvNSZ&0U8l2x_yU4QHpN)ug7kq<&oK8clq7VumVV3B)vm z+AG%>^44)JorDS9O?}QCyGp_*gna|iF8I$HbDOU+FJD^`h$|D7?{$;QM1?1(xax|Z zBe=MIZg2?BVHuLpnb=YRY`}Pr+Y&QWNX*Y8VM!tfV;ob8K*2)-Smx0Gn*%x6iYpb; zh2}$>5m!@tuiN4xi8Vu0+qI>*D)85F+&6ap<2hcl&MX*HHhC&C>KS&uMZvHH0vWoF zDnJc5Ml~xpnpYl4>%!yMW?rsU)7~bJyzqb;xlVdi!qaH?r~$`paq&4s_u`Ozm}98! zOb13?`)bhYt~pNqx7OYjl#P%@W3t^-*)&aEqqWCo@c+M-)Zn5{q9uhj;O!&aVR;#p z0T?T|%z&1z|Dxay3f@)avD>aR-XN5ta{uwI9aa&uX9#X*6jJHz>bJc8&kxnO{7AkU zw<2aw(YSq*TNhjh@Zo~nd<6Ik?vR!XZm+3ZGZj896PY&^vN7t3`I-tN+W2r~g~q*q zjK)=m*=Duu;>Rl*xsmFt=y|+{)2`ivRJ-$uF1wf*BK!gZR9!$Q#;9v^>%^4;d_N=1 z)SpGn8{}6>@eNy2k{$I`Qq`V6pfqn?N!@(W zWMzAY=fMX?x+E?P3V#ZvK5x{3Bf90)_t!7HjQ^`{#pfJ+cDwSUma9;7z6ay*<$4G2 zk#j5dyIjkFpSD~-;oX!sju{@D_+^vurQb0@Be^SE7i22+@Ze7BG!}p_$WBmwx zypaz$juh0K@R67?9+RA1rBaq3!X9(d=>t8N)ESa~B4A<>&-*7{HsyRoqxpgQv;asH zZ>wjx=yHkwU|iSryjaT^f4C9d;u&G zE$CfEVa|RC!_^y-vg;pi4i$huRHsRRxlX%H1k_W_${GCf>M_E8Fd&e#2pFzYoVPL; zGz6%~m3sTkraM$-H(|6ktNHKRxYJ{~Li-kgEjRu}EAS$V|M0bOb|GJGkqspK(&np2$1?}+|*6`hS+2Bi6 z>m_?|FUP2PzU(si6(|G4l^c?-bQe2j4bpq^@0&fu31f&1FH z8U^6jmRnr{!y{m=ob}}|hkbV&xgX=lea)4|r}!U(Toh@k`1az!-p03g59UlTdtzY~ zY|caoMnqnGAx0*aFFs{q_8js@`D(yq$@8paqM2vmhOV~n^4 z0b08LLrRJ~OxTT4QYrK>IV%)c!S#m4^~!Z!xh4HTF&iLjBQ8HNpt}99x1_#RUV{Ob zB+>gR0BSv&%+s>I10JyOczFOcbS+gQLp^*;jFj7_S0jVF*LUYqc3mO-V|T7TLHma7 zjG61#Zb)fYxLneOe3}P4#)}`^J~n9@t$>$AZ3PfUxv)s#Hk|m!0xH|#?4lNa_qY|$ zNdA%^GhfUQCcR+%mp9Fsl@P(M`Q0>?T~{dokWJG|8SsD}gD14p^*N%tz@rtn;WdR= zeYfFqdz*8iymw_U@9sUjFAGmYMjF2BkFVtOVKzVG=*O79p`w4Bj!%En$g5Q0dOPY7=HR%&1Jw( zvznh&j__)YivL>8fpgEZng=H~z);S@E9==5e)jg!cluz*h~OSy*-syL_zV4jHDbu= zBQfB6`X~c_-05S8yScV~g!G{A_EB!%?c@I4KFWZPuU@`=Q~|^GkrShA-|eFe0VXBc z+Nf@SveD6?c$G8lP#_?`g=h@7?x|as`toQU*-X%3)gcY9(fIqG} z|JDEReouw5Zg7QVg#$pn9aMWiYFz)1P6x0{;c3%<*g0(t$(D0M-(pC4ENIcX`k1g}n2U1??a zH(jpZI&l?1J=OX;ajf`UZ}(7c`ym8O4Gms=JYZdX4Ao60m4B0X(;{Y0hab_O>=LJ! z@t3?;-*r(US~3>Zd;~Eej1;!8rxIln3EDAA0I;{ zXEVy9bX`EHX(lE<5aZs23IE>$Mvj%3B-<4B-Bb{N!1t!2GS}amii~{saR>bQ-c+#s zu`;~%redGn3+~l-U6hDFcvJCcuehn$|KWR6!K~}Be|&E$6c}-#=mA$1SgJN+bEF^k z#g&~-qFf^}Q?{7A%Lutg9hZV~&7@!dlb0k{2p)cUQsh;z{U0BX`(dC<+5(yfqT_y~ z;9Ymrr1(7nvo=~oR8z?Cy1EiOj7oX`nc1r~uIswxTlEG7)Uqptsi`<;oL52ETERQL z=1sHeR3m?o28_8KzRd-+ zk1_YsB9p5HBR{+VI6)|CZL_k6{{G_+p6VuyD~qlPqo_;z##lJMHmj6h!QwYsvKR6- z75rW}`b|3=9Kz^3z{do~>AM6tNy0r^?rfhJ4t|#JhBCl{+H1r(5Ri&_n5hJ)Ym>JR zc~v)HWY*s^08L|*QT1V*#Ygr*m^px^F>3hugiTW%qn4NqBa3`WG|p(#UW~IRVGR~H zPC&wk!~s64F_KHdZqpT9+BF@pI6h9e=SqvN68(^N>4G=TDB+_20Y$o~0rg#HJJ-sv z)TVp#^B!x?C^0@-^4f*?%q6e$|MMfgf9u{!hb|JGwYUE%A}j!cJAhmK;`cKFyxoMW zVTVy;lj_&Zf6JaZVPD>_=szrsck#^Lo%7d#P+^PJJ_*K5;WwVxIc-7@DkevzVyb;D z62(-TqMAa8kH0|HErtE+zO(BeNX;)XWk4&&>u-~!eS)R{e5~G#jJ|l?yWT_Z-4wcD z{QXYF{q2;$yXa0vz#}S9lyIHk2C`GP$M_MA&&^B_pT`YvK}qT78l4oMYw7ASM53=k#VN0ovaTg#z&ytI6q7jV;WvcQmF2MjxKkZV3_T*LVj(Ou!#&2^ zu5aTUUbD0jD@LxWlmhT4yra&rC!e!kKs_-`EX<7qRAc;sm@?iFKpnEkYiKLo&P_=EZM@oN?>o=M1Ifue#2Tg`y# zs6n_B1H&HJB*4nyOm%g3c;S*sD?y66vMvQ`tos^@a|-}Qxgnv?Hcsn`Ol84&-z9A%GE9uLG=E{ri>SL>GB?WukbipIZR zoe|a*)l6Y?qYZSBWye_Z?0)Zs_LMVp?5O5sU zaoBP|rsWk_$_xa6I;zL-u8%S9aeOXhXJe;mbqOMb*!0i-cZ|fG)6A#=i=c1{tEps_ z$twT~HHJpVo3WZ=VJeRKu$_m!zCwLLt#;$pITHWwDq3f0E%ajKHDrd7?;JK`*9v`R z!>CazMZg@RzH1N`R|HffYX(5in=P?vbss}t?96al#Q`eTmreDJRi#SqM22skA5exK z;r{zqi9JPeKp5+g2PGSKK8|*2PN~!Y#5Tyz2EK4&_8~9tj(rz|dAME3BvEpQaVrX{ zBA&TwC#w1Guq{wd7ltWj7}34qpAz5)iP_NiXP8Qaj9}Op?h&!CruSgPb6ExiLepM@ z?e!;3a7ib3&FA|?qy(0Nrlr6iYCCxjAS}^WIWptSu@G|9&9ZX@E z!RWny%@lH#L)sWFI*5B5=X@N50dU?FE+BgA1paJz7eGo8N7aqeEkIy6Ih7&D5XTLQ z{iPPEo#DQWR=DvAi)J9hbAugvNMK5CCb(5TZd$-MVt8Qt=>)$R@$$umSN5Oe0~mKw z;g2a7lmzaDLdl+YI09si##5;0`sF?Vscs+YTI~bScD-j*`7I{Amy?p6m(;azkHb@nAF2^&2hH_=y{?Bj z?YVW$9Y6llb`4y)+-;j8T4i&RwIT;f*rYHYgchUxG zs<_K8*ZjMKFQTKsIKx}G27(7wElPYWK3Ui6-0!O2lg^e)(ZVi0VCn=CP9f7eUbH!f z4$6~0?a99sw2oj`TT$LIPF$>O; zyc>S8dY*7|5c$ zr@|38<6amL+|Xv~s@mL8QobG|8Qm?GvABaQP(yAr2zuCEpCW&>Q|N7qTeLn?g|0_o z#8^=40l2f^@gphL&|IM^z5rJPB!-W#nh?s<-2EU^GI_;XdF`t^aSD&c6T7O2=1(dT z!A&8W#`m1UTl~pC1p7%-Se`*Jg~b`HO<`Jy?>mJkGyeUj(2ua*wes9!IQY!iU7?>l zmV3oK-oWJ=_Yq2KeZWgCDSXKTy>1ooYK7E+Vp0VDSSR{AYjqnkrDFRT&bF4 z$no!;#z^Neoc~^(bEXjRkBJWZ>K++T-M+e~!XBmW2~${`ph+1Je|23CSlTn9!#-hM z3p)^5o3GxGn90Zzp(cg8v1RnyLxHE1(W6HxtQp;BQw`tIXD;o$x!)%>IM0zPHalp^=17B@&2aiP#i_bOy$GEC!? zRk%M%=*NC^`UI8T@WDJN&40)vFy~Bidodnj6@RQ3bA~H*N&-CBwqfunNu6pdpe)~g z4Chsa4WmX?(1(*mRbhD#NxQi4TnFkoVqR8Ff*c@0dGIWam8c5&tYD8;EC|^jj8=Sm zYtQUUp^Yx2>An=vG;2#^EtzH!ux4>J1IjDR7}s1k&8$LEH_aL@XkR2fk~&7je8vG1 zcxOXS5UAGK6ru$}yhemoO9y)@Q060PpgFxJaBL-;?r?PZq%mSZn&aO8Txc?~Iq$F7~m+n|% z#L_iArm%r;IpA3WG<2QalX4`nW}%p;sq8vD+rgF)jTPvcx8aR>XwU)v_yp{#)jY*V zTJ$Q0y)JWy5o-nA0+6qM*~zo#l}gHI?ku5P82>UsZUA7a?TZV%rWM_g!p44q=GF## zjeofpL>~3m#Tj(pUM$)yj1fF-%1=S6s}P1MU{rUFqzuJfY?rt|fB%3O5tWp1y-WZ^ zXp(zN-r$Mjm%bi6@yuS`-TER=@Nx&yCU>S~!NL+mv(hWq<~2@0<-XA72F27y%aP8UDgM zil-Q(_K?hoC>yu9e6PdHRka}z6_q|Vk1=?|{LU5JdwJbG>0uDsdAmg#Sx=C5`I_0i zi#b3hS%Sl`0O~HV^Qg~cp=3H-3pg)W2~SPeBQVsBV`dL(5328#`Ve8wh*L<%INE2g zR(oKKh)rG2{dB=~0xuIW5k~$E7MQQGoWfV(c;EoR162z^d(3x4E!C2X5^!UFd{1`% z2-T1RpvE0GHxNj!_iGgjCC|AG8U0g2#Bb{W7TQMeXciL3m zg3paOp*;Xqs=~Ay&^{b%`7nkirCKzvHMH1_;UGO~!Ijr`V7O+KEz%RU9`7z>_*jXk ztdH@vE@T+ZusF)IP*PYfq2!4e<0Dl->vHRpt3y^FAbNui|}Z_N8nhWi}rG zXCU=#ND~fc8W%v<6tXS=ZVGuN)b0+W(KRaoRAZEMN(QLiXXc%+cN&Ee+BWuA8^h>~ ztKAjk-SuTQ#l*ROkRo@jW;n@qDFFVlAnnll-x=E*`(8`fT=&%hichQfI3a+=(seND z!{JiwIu8WMtE1aOIg*TT1MY^S$rUDiiDzpZ`O3JYSWgkvxS4g9-VvYmF2E8pG{Xlb z1YvY&5%5x!hxpL9OhicH7I4|=x<2p9njztFoe2RS6;AVr^W3E z^Eq`E1szhxyO2um4>Lh})C@OhT#!H`4Z~7YzW*`COBT$j>>3A3^Os>c;F%_xK)~s7 zDU#dJM)fL^fH78q2*yf*L&F%!*1ld+K$8eq=$pm3hPY_~uxe=QjE?Huy>{bJ59rB$;7UT^pK+yFE4`#(3c z=gJD>&y<%2KEy?G9A&Dz2CbMZ>bl9p4lhZHfa8^YbJy`dtZ>`9E?O&c*JP(@c|xdi zM$(^<;9V3CXpJm@J;VE<3{Z1z$b#JQ3{!jf`~E+RjK*(}jhlB?nnwG$n7-ffqUab+d7nU6o#-8EW+w`3RI z!k(KR$*N0kyOkn|?n~cnvK6x|h}k7P`PU?7De#Mnweezh1UF|VCgJ8i#mw7p?olX< zn7vFWl9!@l0i4}7RIaskrgtCELRwek2x+-ZDZgxQ!?>aW3V@&C<{s^*KsF1paoaLU zKGw$Va%l9~xP1dKF0SGGko6r2s<1GD|qizs+imqtYEP0gKZK7 zN|YXT*K1{mu?htf)HoR9kTI^Q4;<`rkt4Cf-wwg$c^~$TbBb}|9*4fG=*yY zTdkBmN3aGsvnc3=33^Bj|CqDunA)AbA;x6!tm%Sgc0d&^w+WUzsU0Q&ZWnUq5-=LG zU0}Vm5}^JdKKGbX$Z=+lQp}lo{up0;c9kw@ENYMP@p~kte0_)?IbAD9HRHg@ ziFw^}HBDh;EXc%XW=2U&j7zBlsxexRDWK34xsb{*rWNZ0X?n((Bv)Dz9~C>}7F{&> zJQw}Xzkh$kagVI)0kxVQ*4fx$p2aBb)^&Y8GXbKHiVWcg-_M+6avgWhGTC9&Myekz zCfgrFB@1RWuGC&8=#d*lRn+>S>r|kN+S7s=Ce@ z-dMFL2D?|1CG8l?6;d&E& z*T6?GXwIf@!Kk@pYjC>U%We{z`{8;^>*`@1TyJSz6g(e;;{dMl&)fdLs2D7sk3+@% zAKQ$2DM>SrnCo-6s|1G=|3wZ+*Kk~AV52eG;Uub|uQ>lX#2{44q2}gwQWP2Bl=F2Y zd%$h;;XUNBq@~xq=>x54YuX2 zQ5L; zp%|gs8xm%Nx2()~j)uW23zz<%B|Oz?FLYIF4LKFxTAvazU$rN;**?+{dJl`Bp!RsRJZp6l+oN&l>zh8R5*tPH3 zwcLKb+H6Rh>IC$!G9xcws}pc!g^@}Lc$diPBs}Sdp(!Vdn+i2XEa0h>#68r$^w(vU zR0)-U&nra`b(zSg2KIOdeJC(ev2j0wJ-L=|VNYzuG3rQpb+L`OT$x?Eg+0Ika8CxV z>$Yw_-M<&3ALt(AwtA}|#PMxSS!~5cW49@aGG@dWVhdOIEaMtTU4s&SJP^qg4j2+! zxE6?LB1V-O--^+(iAy`DfaPjCEJhNs+&@NIJHW=C3?oxHU_x-Q+9LglDNv6?k4t89 z1#PauskDEXQM6<bcN&~3Ih<8`n#-%F7*RfCKle{_gN|nOx z2_gGE?qPf0ur}-bV_udVT=h~DjVs%a*^+$my2h(;0LHgmh2cEoGm%HHLh6X?r+P{a z-^^0Dri2faBd%jlI6&68sW(18!*4dUs>nylSGp8){UBl~BScsuWoCWP(beH91M#IQ9|-u=mBVkK5Q8AAonlVQDrlg;c{< zbd_SE05upd@o_=tR&aod37=ApjBqBDrrn&+dG)?~`w6LBZqsCkha$44Q+S-f6`ssq zV;x>vjVs|Br#^UJS7Qi5CXYkM7=>+(dS~-x%Q`S#xXCo-D>z*deF3NRAk(o$SNx7~ z7teJg)yh^p_l-p&HjFX@WZY(5>}jo;x`h$CttR};<$Z`^)><=_-?Et?M0oN~&s9vQ znc6Qs7n~Y7LV9pKgd{zg06krLQcZF`Ye=B_>CzLAd#{w9%Cd^=DYB0T9Cb?y$0+Z` zdYj^dPH_Xzae;+WcxYHc4K8F{W<6`bHj6dt1wd`rEsr~GXDRXXLf>rSgJ+FnoLafK zc!F(W zF&Z)})goy~Q;CtmcNOQesHRYrPX6@4tV7cYNw;jyux~!`WV3gzdJqY99r|JlRqlWq ztzw%U{&AOud69jn3dpIBXHsNrv+Tih0@!1jnc!MHLI?ISm&`*PGbuXmB}CZx-bIv^_^$9F>=i)6dGj_ena4Rm{qGh9u&%gb{+bn$|3+( z#YauFwZ+v17uS-Gpo=GFgGUbjf!WcEE1qZGW^qx;0f34z8k=IS%T%||Ogi98C;SOi z<2z@86!5^= zJ-u?00ip-a;zQYQ^v;>&p8c%I{ZaS!Sm|(wIQK7q*6aE^PBHO)oAvjyJj6y-rl0tClHBz(U^{;M=!f& zvRpv^@~$)+YIGZ-eS=GwFn3; z8w7YO6dE;Id+IhkvD!M<{w%tj(!`@FhjYzLF}}6`gt>l^!LDw|CB+97M253!E@QYd z*~MTC>xp3x)m!$VMcIot`;k3Xj7gUXfJ%%YZE!*9g5i|ik~Ww#^5I&eRR1tuMO;!N zRCSwC$%c+G<(!tsc)Q`zMFsG(%y@M}^#I^=14N}+i%*NXEYuH30R#1l7uv3;7V&0? zxyE}w8+%X%2JgdQseb&61|xlLGz2$aDd3Nm;il(Ct0-rr4gRf)KuAid(%&3XcE{3!6n$&cPOXhkE zO+BVB0xQnSBys*Oj*K~6+G$JkPDxpQgAyYf;78WAvgJ;fem(fQTYKb`hH}K+P3Nr}IyQ20Mwj&%(%UPZptjWZ+LB<>3_$Zt2A zNc;!uOWCs7wv==bulH8;w^fHPvD#H|AN!D(kWCZFK1PL?lpfs`XRJZsmsvs}1Xx>< z?!wynvV85=72Rw^m1$00ORd; zm)o3DsK7f@9;2QZz?WkEM>R&FbdZl1^xCf#V+Bx)5xYnPy1S|NWd z+N+oA@cOv2m?3AHEgO*7M#&%q_l4*!U+yds4M9D7)bc3x3F5=|W_-IKZ>$lg!$@W( zt(o?@7E+~XFb6B)Tsyb|8Y-bExd3nB+0m;9ted&7j4Y_QGh_1WgzVIK7nckLbRB2E zi&MA2FRPHWn3SFDb z$70uK%*PVo%fj)Pm)n!yvvF~$jp4F}NPshP#E*z*?`U2|P@Qy1-+2bHXBF?fn=tzI z#mg`NJf8<0&PrL;B-bAQoA7H>04HcO-az65cPj(@osB1s(G8_AGZ(q=)M#z?Io?92 zQO0--@tKFYCb@;c-B2{j`->$j@qq3r`0UVN3@Z&yaF0_ovYDDLSFTdaOG~vJVluwJ zKnh-igVL+0 z8spw(meIomDaJ_CxjZKix`Y#BJ{JfCTSYU!^IZ6dyJaw@NfzGc@h@K}UC0)UI$~^m zBs8yi&xS`<_01F?p5|IOm9DD=P)UrQJ+I>%u_d~t%v$_QZ~$e`^@UE@1h~l`tmNBJc=ppprw@DXcG8>WUXna#H}iBQ#Rkg6o}x#NczxjjEKFX5@gn9kR31 za1Dq-7%AEte;Ve_*JX?;`f5_i?Iqo!%5b{c%2pk|R(NQ~DDiQd4EUUCjU2|$JEn}d zW)qJoWPvrh?9xm#fK3yeBk1az0)m^^?Fz|8Q%&~ y$1koWz8^^J?ok*A6qF3{IT(!{{_n45oT7o#QuPU{i?0000 + + + + + + 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:2368369464.1 %
Date:2024-04-09 22:03:33Functions:8912372.4 %
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 +
63.7%63.7%
+ 		63.7 %2325 / 365169.4 %77 / 111
<unnamed>63.7 %2325 / 365169.4 %77 / 111
output_publisher.cpp +
100.0%
+ 		100.0 %43 / 43100.0 %12 / 12
<unnamed>100.0 %43 / 43100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/index-detail-sort-l.html b/mrs_uav_managers/src/control_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..f33e202605 --- /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:2368369464.1 %
Date:2024-04-09 22:03:33Functions:8912372.4 %
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 +
63.7%63.7%
+ 		63.7 %2325 / 365169.4 %77 / 111
<unnamed>63.7 %2325 / 365169.4 %77 / 111
output_publisher.cpp +
100.0%
+ 		100.0 %43 / 43100.0 %12 / 12
<unnamed>100.0 %43 / 43100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/index-detail.html b/mrs_uav_managers/src/control_manager/index-detail.html new file mode 100644 index 0000000000..61ccb0f705 --- /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:2368369464.1 %
Date:2024-04-09 22:03:33Functions:8912372.4 %
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 +
63.7%63.7%
+ 		63.7 %2325 / 365169.4 %77 / 111
<unnamed>63.7 %2325 / 365169.4 %77 / 111
output_publisher.cpp +
100.0%
+ 		100.0 %43 / 43100.0 %12 / 12
<unnamed>100.0 %43 / 43100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/index-sort-f.html b/mrs_uav_managers/src/control_manager/index-sort-f.html new file mode 100644 index 0000000000..ed4c4985f5 --- /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:2368369464.1 %
Date:2024-04-09 22:03:33Functions:8912372.4 %
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 +
63.7%63.7%
+ 		63.7 %2325 / 365169.4 %77 / 111
output_publisher.cpp +
100.0%
+ 		100.0 %43 / 43100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/index-sort-l.html b/mrs_uav_managers/src/control_manager/index-sort-l.html new file mode 100644 index 0000000000..120572624e --- /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:2368369464.1 %
Date:2024-04-09 22:03:33Functions:8912372.4 %
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 +
63.7%63.7%
+ 		63.7 %2325 / 365169.4 %77 / 111
output_publisher.cpp +
100.0%
+ 		100.0 %43 / 43100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/index.html b/mrs_uav_managers/src/control_manager/index.html new file mode 100644 index 0000000000..8dbb10ad60 --- /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:2368369464.1 %
Date:2024-04-09 22:03:33Functions:8912372.4 %
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 +
63.7%63.7%
+ 		63.7 %2325 / 365169.4 %77 / 111
output_publisher.cpp +
100.0%
+ 		100.0 %43 / 43100.0 %12 / 12
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/control_manager/output_publisher.cpp.func-sort-c.html b/mrs_uav_managers/src/control_manager/output_publisher.cpp.func-sort-c.html new file mode 100644 index 0000000000..209325c659 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/output_publisher.cpp.func-sort-c.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/output_publisher.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager - output_publisher.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4343100.0 %
Date:2024-04-09 22:03:33Functions: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&)82
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher()82
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)477
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)997
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1042
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1181
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2253
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiActuatorCmd_<std::allocator<void> > const&)3816
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3845
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)7897
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)78813
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&)100321
+
+
+ + + +
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..00b712dcf5 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/output_publisher.cpp.func.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/output_publisher.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager - output_publisher.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4343100.0 %
Date:2024-04-09 22:03:33Functions: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&)3816
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeCmd_<std::allocator<void> > const&)7897
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiPositionCmd_<std::allocator<void> > const&)477
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgCmd_<std::allocator<void> > const&)1181
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAttitudeRateCmd_<std::allocator<void> > const&)78813
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiControlGroupCmd_<std::allocator<void> > const&)3845
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgCmd_<std::allocator<void> > const&)1042
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiVelocityHdgRateCmd_<std::allocator<void> > const&)2253
mrs_uav_managers::control_manager::OutputPublisher::publish(mrs_msgs::HwApiAccelerationHdgRateCmd_<std::allocator<void> > const&)997
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&)100321
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher(ros::NodeHandle&)82
mrs_uav_managers::control_manager::OutputPublisher::OutputPublisher()82
+
+
+ + + +
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..6c65e945e5 --- /dev/null +++ b/mrs_uav_managers/src/control_manager/output_publisher.cpp.gcov.html @@ -0,0 +1,154 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/control_manager/output_publisher.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/control_manager - output_publisher.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:4343100.0 %
Date:2024-04-09 22:03:33Functions: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          82 : OutputPublisher::OutputPublisher() {
+      10          82 : }
+      11             : 
+      12          82 : OutputPublisher::OutputPublisher(ros::NodeHandle& nh) {
+      13             : 
+      14          82 :   ph_hw_api_actuator_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiActuatorCmd>(nh, "hw_api_actuator_cmd_out", 1);
+      15          82 :   ph_hw_api_control_group_cmd_         = mrs_lib::PublisherHandler<mrs_msgs::HwApiControlGroupCmd>(nh, "hw_api_control_group_cmd_out", 1);
+      16          82 :   ph_hw_api_attitude_rate_cmd_         = mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeRateCmd>(nh, "hw_api_attitude_rate_cmd_out", 1);
+      17          82 :   ph_hw_api_attitude_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiAttitudeCmd>(nh, "hw_api_attitude_cmd_out", 1);
+      18          82 :   ph_hw_api_acceleration_hdg_rate_cmd_ = mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgRateCmd>(nh, "hw_api_acceleration_hdg_rate_cmd_out", 1);
+      19          82 :   ph_hw_api_acceleration_hdg_cmd_      = mrs_lib::PublisherHandler<mrs_msgs::HwApiAccelerationHdgCmd>(nh, "hw_api_acceleration_hdg_cmd_out", 1);
+      20          82 :   ph_hw_api_velocity_hdg_rate_cmd_     = mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgRateCmd>(nh, "hw_api_velocity_hdg_rate_cmd_out", 1);
+      21          82 :   ph_hw_api_velocity_hdg_cmd_          = mrs_lib::PublisherHandler<mrs_msgs::HwApiVelocityHdgCmd>(nh, "hw_api_velocity_hdg_cmd_out", 1);
+      22          82 :   ph_hw_api_position_cmd_              = mrs_lib::PublisherHandler<mrs_msgs::HwApiPositionCmd>(nh, "hw_api_position_cmd_out", 1);
+      23          82 : }
+      24             : 
+      25      100321 : void OutputPublisher::publish(const Controller::HwApiOutputVariant& control_output) {
+      26             : 
+      27      100321 :   std::visit(OutputPublisher::PublisherVisitor(this), control_output);
+      28      100321 : }
+      29             : 
+      30             : // | ------------------------- private ------------------------ |
+      31             : 
+      32        3816 : void OutputPublisher::publish(const mrs_msgs::HwApiActuatorCmd& msg) {
+      33        3816 :   ph_hw_api_actuator_cmd_.publish(msg);
+      34        3816 : }
+      35             : 
+      36        3845 : void OutputPublisher::publish(const mrs_msgs::HwApiControlGroupCmd& msg) {
+      37        3845 :   ph_hw_api_control_group_cmd_.publish(msg);
+      38        3845 : }
+      39             : 
+      40       78813 : void OutputPublisher::publish(const mrs_msgs::HwApiAttitudeRateCmd& msg) {
+      41       78813 :   ph_hw_api_attitude_rate_cmd_.publish(msg);
+      42       78813 : }
+      43             : 
+      44        7897 : void OutputPublisher::publish(const mrs_msgs::HwApiAttitudeCmd& msg) {
+      45        7897 :   ph_hw_api_attitude_cmd_.publish(msg);
+      46        7897 : }
+      47             : 
+      48         997 : void OutputPublisher::publish(const mrs_msgs::HwApiAccelerationHdgRateCmd& msg) {
+      49         997 :   ph_hw_api_acceleration_hdg_rate_cmd_.publish(msg);
+      50         997 : }
+      51             : 
+      52        1042 : void OutputPublisher::publish(const mrs_msgs::HwApiAccelerationHdgCmd& msg) {
+      53        1042 :   ph_hw_api_acceleration_hdg_cmd_.publish(msg);
+      54        1042 : }
+      55             : 
+      56        2253 : void OutputPublisher::publish(const mrs_msgs::HwApiVelocityHdgRateCmd& msg) {
+      57        2253 :   ph_hw_api_velocity_hdg_rate_cmd_.publish(msg);
+      58        2253 : }
+      59             : 
+      60        1181 : void OutputPublisher::publish(const mrs_msgs::HwApiVelocityHdgCmd& msg) {
+      61        1181 :   ph_hw_api_velocity_hdg_cmd_.publish(msg);
+      62        1181 : }
+      63             : 
+      64         477 : void OutputPublisher::publish(const mrs_msgs::HwApiPositionCmd& msg) {
+      65         477 :   ph_hw_api_position_cmd_.publish(msg);
+      66         477 : }
+      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..57cef5f4d2 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func-sort-c.html @@ -0,0 +1,176 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimation_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimation_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:37153469.5 %
Date:2024-04-09 22:03:33Functions:212487.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::estimation_manager::EstimationManager::loadConfigFile(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
mrs_uav_managers::estimation_manager::EstimationManager::callFailsafeService()0
mrs_uav_managers::estimation_manager::EstimationManager::switchToHealthyEstimator()0
mrs_uav_managers::estimation_manager::StateMachine::changeToPreSwitchState()4
mrs_uav_managers::estimation_manager::EstimationManager::switchToEstimator(boost::shared_ptr<mrs_uav_managers::StateEstimator> const&)4
mrs_uav_managers::estimation_manager::EstimationManager::callbackChangeEstimator(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)4
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_managers::estimation_manager::StateMachine::StateMachine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)82
mrs_uav_managers::estimation_manager::EstimationManager::timerInitialization(ros::TimerEvent const&)82
mrs_uav_managers::estimation_manager::EstimationManager::onInit()82
mrs_uav_managers::estimation_manager::EstimationManager::EstimationManager()82
mrs_uav_managers::estimation_manager::EstimationManager::callbackToggleServiceCallbacks(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)113
mrs_uav_managers::estimation_manager::StateMachine::changeState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&)265
mrs_uav_managers::estimation_manager::StateMachine::getPrintName[abi:cxx11]() const265
mrs_uav_managers::estimation_manager::StateMachine::getStateAsString[abi:cxx11](mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const530
mrs_uav_managers::estimation_manager::EstimationManager::getName[abi:cxx11]() const2779
mrs_uav_managers::estimation_manager::StateMachine::getCurrentStateString[abi:cxx11]() const16124
mrs_uav_managers::estimation_manager::EstimationManager::timerPublishDiagnostics(ros::TimerEvent const&)16981
mrs_uav_managers::estimation_manager::StateMachine::isInTheAir() const161735
mrs_uav_managers::estimation_manager::EstimationManager::timerPublish(ros::TimerEvent const&)161736
mrs_uav_managers::estimation_manager::EstimationManager::timerCheckHealth(ros::TimerEvent const&)161740
mrs_uav_managers::estimation_manager::StateMachine::isInPublishableState() const178708
mrs_uav_managers::estimation_manager::StateMachine::isInitialized() const340484
mrs_uav_managers::estimation_manager::StateMachine::isInState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const1551497
+
+
+ + + +
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..73ada768c1 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.func.html @@ -0,0 +1,176 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimation_manager.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager - estimation_manager.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:37153469.5 %
Date:2024-04-09 22:03:33Functions:212487.5 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_managers::estimation_manager::StateMachine::changeState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&)265
mrs_uav_managers::estimation_manager::StateMachine::changeToPreSwitchState()4
mrs_uav_managers::estimation_manager::StateMachine::StateMachine(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)82
mrs_uav_managers::estimation_manager::EstimationManager::timerPublish(ros::TimerEvent const&)161736
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&)161740
mrs_uav_managers::estimation_manager::EstimationManager::switchToEstimator(boost::shared_ptr<mrs_uav_managers::StateEstimator> const&)4
mrs_uav_managers::estimation_manager::EstimationManager::callFailsafeService()0
mrs_uav_managers::estimation_manager::EstimationManager::timerInitialization(ros::TimerEvent const&)82
mrs_uav_managers::estimation_manager::EstimationManager::callbackChangeEstimator(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)4
mrs_uav_managers::estimation_manager::EstimationManager::timerPublishDiagnostics(ros::TimerEvent const&)16981
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> >&)113
mrs_uav_managers::estimation_manager::EstimationManager::onInit()82
mrs_uav_managers::estimation_manager::EstimationManager::EstimationManager()82
mrs_uav_managers::estimation_manager::StateMachine::isInTheAir() const161735
mrs_uav_managers::estimation_manager::StateMachine::getPrintName[abi:cxx11]() const265
mrs_uav_managers::estimation_manager::StateMachine::isInitialized() const340484
mrs_uav_managers::estimation_manager::StateMachine::getStateAsString[abi:cxx11](mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const530
mrs_uav_managers::estimation_manager::StateMachine::isInPublishableState() const178708
mrs_uav_managers::estimation_manager::StateMachine::getCurrentStateString[abi:cxx11]() const16124
mrs_uav_managers::estimation_manager::StateMachine::isInState(mrs_uav_managers::estimation_manager::StateMachine::SMState_t const&) const1551497
mrs_uav_managers::estimation_manager::EstimationManager::getName[abi:cxx11]() const2779
+
+
+ + + +
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..447bb6cd65 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.html @@ -0,0 +1,1330 @@ + + + + + + + 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:37153469.5 %
Date:2024-04-09 22:03:33Functions:212487.5 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /*//{ includes */
+       2             : #include <ros/ros.h>
+       3             : #include <nodelet/nodelet.h>
+       4             : 
+       5             : #include <pluginlib/class_loader.h>
+       6             : #include <nodelet/loader.h>
+       7             : 
+       8             : #include <geometry_msgs/QuaternionStamped.h>
+       9             : #include <geometry_msgs/Vector3Stamped.h>
+      10             : 
+      11             : #include <nav_msgs/Odometry.h>
+      12             : 
+      13             : #include <std_srvs/Trigger.h>
+      14             : #include <std_srvs/SetBool.h>
+      15             : 
+      16             : #include <mrs_msgs/UavState.h>
+      17             : #include <mrs_msgs/Float64Stamped.h>
+      18             : #include <mrs_msgs/String.h>
+      19             : #include <mrs_msgs/EstimationDiagnostics.h>
+      20             : #include <mrs_msgs/HwApiCapabilities.h>
+      21             : #include <mrs_msgs/ControlManagerDiagnostics.h>
+      22             : 
+      23             : #include <mrs_lib/param_loader.h>
+      24             : #include <mrs_lib/publisher_handler.h>
+      25             : #include <mrs_lib/service_client_handler.h>
+      26             : #include <mrs_lib/transformer.h>
+      27             : #include <mrs_lib/subscribe_handler.h>
+      28             : #include <mrs_lib/gps_conversions.h>
+      29             : #include <mrs_lib/scope_timer.h>
+      30             : 
+      31             : 
+      32             : #include <mrs_uav_managers/state_estimator.h>
+      33             : #include <mrs_uav_managers/agl_estimator.h>
+      34             : #include <mrs_uav_managers/estimation_manager/support.h>
+      35             : #include <mrs_uav_managers/estimation_manager/common_handlers.h>
+      36             : #include <mrs_uav_managers/estimation_manager/private_handlers.h>
+      37             : /*//}*/
+      38             : 
+      39             : namespace mrs_uav_managers
+      40             : {
+      41             : 
+      42             : namespace estimation_manager
+      43             : {
+      44             : 
+      45             : // --------------------------------------------------------------
+      46             : // |                           classes                          |
+      47             : // --------------------------------------------------------------
+      48             : 
+      49             : /*//{ class StateMachine */
+      50             : class StateMachine {
+      51             : 
+      52             :   /*//{ states */
+      53             : public:
+      54             :   typedef enum
+      55             :   {
+      56             : 
+      57             :     UNINITIALIZED_STATE,
+      58             :     INITIALIZED_STATE,
+      59             :     READY_FOR_FLIGHT_STATE,
+      60             :     TAKING_OFF_STATE,
+      61             :     FLYING_STATE,
+      62             :     HOVER_STATE,
+      63             :     LANDING_STATE,
+      64             :     LANDED_STATE,
+      65             :     ESTIMATOR_SWITCHING_STATE,
+      66             :     DUMMY_STATE,
+      67             :     EMERGENCY_STATE,
+      68             :     FAILSAFE_STATE,
+      69             :     ERROR_STATE
+      70             : 
+      71             :   } SMState_t;
+      72             : 
+      73             :   /*//}*/
+      74             : 
+      75             : public:
+      76        1148 :   StateMachine(const std::string& nodelet_name) : nodelet_name_(nodelet_name) {
+      77          82 :   }
+      78             : 
+      79     1551497 :   bool isInState(const SMState_t& state) const {
+      80     1551497 :     return mrs_lib::get_mutexed(mtx_state_, current_state_) == state;
+      81             :   }
+      82             : 
+      83      340484 :   bool isInitialized() const {
+      84      340484 :     return mrs_lib::get_mutexed(mtx_state_, current_state_) != UNINITIALIZED_STATE;
+      85             :   }
+      86             : 
+      87      178708 :   bool isInPublishableState() const {
+      88      178708 :     const SMState_t current_state = mrs_lib::get_mutexed(mtx_state_, current_state_);
+      89      135479 :     return current_state == READY_FOR_FLIGHT_STATE || current_state == TAKING_OFF_STATE || current_state == HOVER_STATE || current_state == FLYING_STATE ||
+      90      314191 :            current_state == LANDING_STATE || current_state == DUMMY_STATE || current_state == FAILSAFE_STATE;
+      91             :   }
+      92             : 
+      93      161735 :   bool isInTheAir() const {
+      94      161735 :     const SMState_t current_state = mrs_lib::get_mutexed(mtx_state_, current_state_);
+      95      161735 :     return current_state == TAKING_OFF_STATE || current_state == HOVER_STATE || current_state == FLYING_STATE || current_state == LANDING_STATE;
+      96             :   }
+      97             : 
+      98             :   SMState_t getCurrentState() const {
+      99             :     return mrs_lib::get_mutexed(mtx_state_, current_state_);
+     100             :   }
+     101             : 
+     102       16124 :   std::string getCurrentStateString() const {
+     103       16124 :     return mrs_lib::get_mutexed(mtx_state_, sm_state_names_[current_state_]);
+     104             :   }
+     105             : 
+     106         530 :   std::string getStateAsString(const SMState_t& state) const {
+     107         530 :     return sm_state_names_[state];
+     108             :   }
+     109             : 
+     110             :   /*//{ changeState() */
+     111         265 :   bool changeState(const SMState_t& target_state) {
+     112             : 
+     113         265 :     if (target_state == current_state_) {
+     114             : 
+     115           0 :       ROS_WARN("[%s]: requested change to same state %s -> %s", getPrintName().c_str(), getStateAsString(current_state_).c_str(),
+     116             :                getStateAsString(target_state).c_str());
+     117           0 :       return true;
+     118             :     }
+     119             : 
+     120         265 :     switch (target_state) {
+     121             : 
+     122           0 :       case UNINITIALIZED_STATE: {
+     123           0 :         ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is not possible from any state", getPrintName().c_str(), getStateAsString(UNINITIALIZED_STATE).c_str());
+     124           0 :         return false;
+     125             :         break;
+     126             :       }
+     127             : 
+     128          82 :       case INITIALIZED_STATE: {
+     129          82 :         if (current_state_ != UNINITIALIZED_STATE) {
+     130           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s", getPrintName().c_str(), getStateAsString(INITIALIZED_STATE).c_str(),
+     131             :                              getStateAsString(UNINITIALIZED_STATE).c_str());
+     132           0 :           return false;
+     133             :         }
+     134          82 :         break;
+     135             :       }
+     136             : 
+     137          82 :       case READY_FOR_FLIGHT_STATE: {
+     138          82 :         if (current_state_ != INITIALIZED_STATE && current_state_ != LANDED_STATE) {
+     139           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s or %s", getPrintName().c_str(),
+     140             :                              getStateAsString(READY_FOR_FLIGHT_STATE).c_str(), getStateAsString(INITIALIZED_STATE).c_str(),
+     141             :                              getStateAsString(LANDED_STATE).c_str());
+     142           0 :           return false;
+     143             :         }
+     144          82 :         break;
+     145             :       }
+     146             : 
+     147          19 :       case TAKING_OFF_STATE: {
+     148          19 :         if (current_state_ != READY_FOR_FLIGHT_STATE) {
+     149           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s", getPrintName().c_str(), getStateAsString(TAKING_OFF_STATE).c_str(),
+     150             :                              getStateAsString(READY_FOR_FLIGHT_STATE).c_str());
+     151           0 :           return false;
+     152             :         }
+     153          19 :         break;
+     154             :       }
+     155             : 
+     156          78 :       case FLYING_STATE: {
+     157          78 :         if (current_state_ != TAKING_OFF_STATE && current_state_ != READY_FOR_FLIGHT_STATE && current_state_ != HOVER_STATE && current_state_ != ESTIMATOR_SWITCHING_STATE) {
+     158           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(),
+     159             :                              getStateAsString(TAKING_OFF_STATE).c_str(), getStateAsString(HOVER_STATE).c_str(),
+     160             :                              getStateAsString(ESTIMATOR_SWITCHING_STATE).c_str());
+     161           0 :           return false;
+     162             :         }
+     163          78 :         break;
+     164             :       }
+     165             : 
+     166           0 :       case HOVER_STATE: {
+     167           0 :         if (current_state_ != FLYING_STATE) {
+     168           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s", getPrintName().c_str(), getStateAsString(HOVER_STATE).c_str(),
+     169             :                              getStateAsString(FLYING_STATE).c_str());
+     170           0 :           return false;
+     171             :         }
+     172           0 :         break;
+     173             :       }
+     174             : 
+     175           4 :       case ESTIMATOR_SWITCHING_STATE: {
+     176           4 :         if (current_state_ != FLYING_STATE && current_state_ != HOVER_STATE) {
+     177           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s or %s", getPrintName().c_str(),
+     178             :                              getStateAsString(ESTIMATOR_SWITCHING_STATE).c_str(), getStateAsString(FLYING_STATE).c_str(),
+     179             :                              getStateAsString(HOVER_STATE).c_str());
+     180           0 :           return false;
+     181             :         }
+     182           4 :         pre_switch_state_ = current_state_;
+     183           4 :         break;
+     184             :       }
+     185             : 
+     186           0 :       case LANDING_STATE: {
+     187           0 :         if (current_state_ != FLYING_STATE && current_state_ != HOVER_STATE) {
+     188           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s or %s", getPrintName().c_str(), getStateAsString(LANDING_STATE).c_str(),
+     189             :                              getStateAsString(FLYING_STATE).c_str(), getStateAsString(HOVER_STATE).c_str());
+     190           0 :           return false;
+     191             :         }
+     192           0 :         break;
+     193             :       }
+     194             : 
+     195           0 :       case LANDED_STATE: {
+     196           0 :         if (current_state_ != LANDING_STATE) {
+     197           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s", getPrintName().c_str(), getStateAsString(LANDED_STATE).c_str(),
+     198             :                              getStateAsString(LANDING_STATE).c_str());
+     199           0 :           return false;
+     200             :         }
+     201           0 :         break;
+     202             :       }
+     203             : 
+     204           0 :       case DUMMY_STATE: {
+     205           0 :         if (current_state_ != INITIALIZED_STATE) {
+     206           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: transition to %s is possible only from %s", getPrintName().c_str(), getStateAsString(DUMMY_STATE).c_str(),
+     207             :                              getStateAsString(INITIALIZED_STATE).c_str());
+     208           0 :           return false;
+     209             :         }
+     210           0 :         break;
+     211             :       }
+     212           0 :       case EMERGENCY_STATE: {
+     213           0 :         ROS_WARN("[%s]: transition to %s", getPrintName().c_str(), getStateAsString(EMERGENCY_STATE).c_str());
+     214           0 :         break;
+     215             :       }
+     216             : 
+     217           0 :       case ERROR_STATE: {
+     218           0 :         ROS_WARN("[%s]: transition to %s", getPrintName().c_str(), getStateAsString(ERROR_STATE).c_str());
+     219           0 :         break;
+     220             :       }
+     221             : 
+     222           0 :       case FAILSAFE_STATE: {
+     223           0 :         ROS_WARN("[%s]: transition to %s", getPrintName().c_str(), getStateAsString(FAILSAFE_STATE).c_str());
+     224           0 :         break;
+     225             :       }
+     226             : 
+     227           0 :       default: {
+     228           0 :         ROS_ERROR_THROTTLE(1.0, "[%s]: rejected transition to unknown state id %d", getPrintName().c_str(), target_state);
+     229           0 :         return false;
+     230             :         break;
+     231             :       }
+     232             :     }
+     233             : 
+     234         265 :     std::scoped_lock lock(mtx_state_);
+     235             :     {
+     236         265 :       previous_state_ = current_state_;
+     237         265 :       current_state_  = target_state;
+     238             :     }
+     239             : 
+     240         265 :     ROS_INFO("[%s]: successfully changed states %s -> %s", getPrintName().c_str(), getStateAsString(previous_state_).c_str(),
+     241             :              getStateAsString(current_state_).c_str());
+     242             : 
+     243         265 :     return true;
+     244             :   }
+     245             :   /*//}*/
+     246             : 
+     247             :   /*//{ changeToPreSwitchState() */
+     248           4 :   void changeToPreSwitchState() {
+     249           4 :     changeState(pre_switch_state_);
+     250           4 :   }
+     251             :   /*//}*/
+     252             : 
+     253             : private:
+     254             :   const std::string name_ = "StateMachine";
+     255             :   const std::string nodelet_name_;
+     256             : 
+     257             :   SMState_t current_state_    = UNINITIALIZED_STATE;
+     258             :   SMState_t previous_state_   = UNINITIALIZED_STATE;
+     259             :   SMState_t pre_switch_state_ = UNINITIALIZED_STATE;
+     260             : 
+     261             :   mutable std::mutex mtx_state_;
+     262             : 
+     263             :   std::string getName() const {
+     264             :     return name_;
+     265             :   }
+     266             : 
+     267         265 :   std::string getPrintName() const {
+     268         530 :     return nodelet_name_ + "/" + name_;
+     269             :   }
+     270             : 
+     271             :   // clang-format off
+     272             :   const std::vector<std::string> sm_state_names_ = {
+     273             :   "UNINITIALIZED_STATE",
+     274             :   "INITIALIZED_STATE",
+     275             :   "READY_FOR_FLIGHT_STATE",
+     276             :   "TAKING_OFF_STATE",
+     277             :   "FLYING_STATE",
+     278             :   "HOVER_STATE",
+     279             :   "LANDING_STATE",
+     280             :   "LANDED_STATE",
+     281             :   "ESTIMATOR_SWITCHING_STATE",
+     282             :   "DUMMY_STATE",
+     283             :   "EMERGENCY_STATE",
+     284             :   "FAILSAFE_STATE",
+     285             :   "ERROR_STATE"
+     286             :   };
+     287             :   // clang-format on
+     288             : };
+     289             : /*//}*/
+     290             : 
+     291             : /*//{ class EstimationManager */
+     292             : class EstimationManager : public nodelet::Nodelet {
+     293             : 
+     294             : private:
+     295             :   const std::string nodelet_name_ = "EstimationManager";
+     296             :   const std::string package_name_ = "mrs_uav_managers";
+     297             : 
+     298             :   ros::NodeHandle nh_;
+     299             : 
+     300             :   std::string _custom_config_;
+     301             :   std::string _platform_config_;
+     302             :   std::string _world_config_;
+     303             : 
+     304             :   std::shared_ptr<CommonHandlers_t> ch_;
+     305             : 
+     306             :   std::shared_ptr<StateMachine> sm_;
+     307             : 
+     308             :   std::string after_midair_activation_tracker_name_;
+     309             :   std::string takeoff_tracker_name_;
+     310             : 
+     311             :   mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics> sh_control_manager_diag_;
+     312             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>            sh_control_input_;
+     313             : 
+     314             :   mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics> ph_diagnostics_;
+     315             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>        ph_max_flight_z_;
+     316             :   mrs_lib::PublisherHandler<mrs_msgs::UavState>              ph_uav_state_;
+     317             :   mrs_lib::PublisherHandler<nav_msgs::Odometry>              ph_odom_main_;
+     318             :   mrs_lib::PublisherHandler<nav_msgs::Odometry>              ph_odom_slow_;  // use topic throttler instead?
+     319             :   mrs_lib::PublisherHandler<nav_msgs::Odometry>              ph_innovation_;
+     320             : 
+     321             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped> ph_altitude_amsl_;
+     322             :   mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped> ph_altitude_agl_;
+     323             : 
+     324             :   mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped> ph_orientation_;
+     325             : 
+     326             :   ros::Timer timer_publish_;
+     327             :   void       timerPublish(const ros::TimerEvent& event);
+     328             : 
+     329             :   ros::Timer timer_publish_diagnostics_;
+     330             :   void       timerPublishDiagnostics(const ros::TimerEvent& event);
+     331             : 
+     332             :   ros::Timer timer_check_health_;
+     333             :   void       timerCheckHealth(const ros::TimerEvent& event);
+     334             : 
+     335             :   ros::Timer timer_initialization_;
+     336             :   void       timerInitialization(const ros::TimerEvent& event);
+     337             : 
+     338             :   ros::ServiceServer srvs_change_estimator_;
+     339             :   bool               callbackChangeEstimator(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+     340             :   int                estimator_switch_count_ = 0;
+     341             : 
+     342             : 
+     343             :   ros::ServiceServer srvs_toggle_callbacks_;
+     344             :   bool               callbackToggleServiceCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     345             :   bool               callbacks_enabled_             = false;
+     346             :   bool               callbacks_disabled_by_service_ = false;
+     347             : 
+     348             :   bool                                             callFailsafeService();
+     349             :   mrs_lib::ServiceClientHandler<std_srvs::Trigger> srvch_failsafe_;
+     350             :   bool                                             failsafe_call_succeeded_ = false;
+     351             : 
+     352             :   // TODO service clients
+     353             :   /* mrs_lib::ServiceClientHandler<std_srvs::Trigger> srvc_hover_; */
+     354             :   /* mrs_lib::ServiceClientHandler<mrs_msgs::ReferenceStampedSrv> srvc_reference_; */
+     355             :   /* mrs_lib::ServiceClientHandler<std_srvs::Trigger> srvc_ehover_; */
+     356             :   /* mrs_lib::ServiceClientHandler<std_srvs::SetBool> srvc_enable_callbacks_; */
+     357             : 
+     358             :   // | ------------- dynamic loading of estimators ------------- |
+     359             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::StateEstimator>> state_estimator_loader_;  // pluginlib loader of dynamically loaded estimators
+     360             :   std::vector<std::string>                                                  estimator_names_;         // list of estimator names
+     361             :   std::vector<boost::shared_ptr<mrs_uav_managers::StateEstimator>>          estimator_list_;          // list of estimators
+     362             :   std::mutex                                                                mutex_estimator_list_;
+     363             :   std::vector<std::string>                                                  switchable_estimator_names_;
+     364             :   std::mutex                                                                mutex_switchable_estimator_names_;
+     365             :   std::string                                                               initial_estimator_name_ = "UNDEFINED_INITIAL_ESTIMATOR";
+     366             :   boost::shared_ptr<mrs_uav_managers::StateEstimator>                       initial_estimator_;
+     367             :   boost::shared_ptr<mrs_uav_managers::StateEstimator>                       active_estimator_;
+     368             :   std::mutex                                                                mutex_active_estimator_;
+     369             : 
+     370             :   std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::AglEstimator>> agl_estimator_loader_;  // pluginlib loader of dynamically loaded estimators
+     371             :   std::string                                                             est_alt_agl_name_ = "UNDEFINED_AGL_ESTIMATOR";
+     372             :   boost::shared_ptr<mrs_uav_managers::AglEstimator>                       est_alt_agl_;
+     373             :   bool                                                                    is_using_agl_estimator_;
+     374             : 
+     375             :   double max_flight_z_;
+     376             : 
+     377             :   bool switchToHealthyEstimator();
+     378             :   void switchToEstimator(const boost::shared_ptr<mrs_uav_managers::StateEstimator>& target_estimator);
+     379             : 
+     380             :   bool loadConfigFile(const std::string& file_path);
+     381             : 
+     382             : public:
+     383          82 :   EstimationManager() {
+     384          82 :   }
+     385             : 
+     386             :   void onInit();
+     387             : 
+     388             :   std::string getName() const;
+     389             : };
+     390             : /*//}*/
+     391             : 
+     392             : /*//{ onInit() */
+     393          82 : void EstimationManager::onInit() {
+     394             : 
+     395          82 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     396             : 
+     397          82 :   ros::Time::waitForValid();
+     398             : 
+     399          82 :   ROS_INFO("[%s]: initializing", getName().c_str());
+     400             : 
+     401          82 :   sm_ = std::make_shared<StateMachine>(nodelet_name_);
+     402             : 
+     403          82 :   ch_ = std::make_shared<CommonHandlers_t>();
+     404             : 
+     405          82 :   ch_->nodelet_name = nodelet_name_;
+     406          82 :   ch_->package_name = package_name_;
+     407             : 
+     408             :   // finish initialization in a oneshot timer, so that we don't block loading of other nodelets by the nodelet manager
+     409          82 :   timer_initialization_ = nh_.createTimer(ros::Rate(1.0), &EstimationManager::timerInitialization, this, true, true);
+     410          82 : }
+     411             : /*//}*/
+     412             : 
+     413             : // --------------------------------------------------------------
+     414             : // |                          callbacks                         |
+     415             : // --------------------------------------------------------------
+     416             : 
+     417             : // | --------------------- timer callbacks -------------------- |
+     418             : 
+     419             : /*//{ timerPublish() */
+     420      161736 : void EstimationManager::timerPublish([[maybe_unused]] const ros::TimerEvent& event) {
+     421             : 
+     422      161736 :   if (!sm_->isInitialized()) {
+     423       16016 :     return;
+     424             :   }
+     425             : 
+     426      323462 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("EstimationManager::timerPublish", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     427             : 
+     428      161731 :   if (sm_->isInState(StateMachine::ESTIMATOR_SWITCHING_STATE)) {
+     429           0 :     ROS_WARN("[%s]: Not publishing during estimator switching.", getName().c_str());
+     430           0 :     return;
+     431             :   }
+     432             : 
+     433      161731 :   if (!sm_->isInPublishableState()) {
+     434       16011 :     ROS_WARN_THROTTLE(1.0, "[%s]: not publishing uav state in %s", getName().c_str(), sm_->getCurrentStateString().c_str());
+     435       16011 :     return;
+     436             :   }
+     437             : 
+     438      145720 :   mrs_msgs::UavState uav_state;
+     439      145720 :   auto               ret = active_estimator_->getUavState();
+     440      145720 :   if (ret) {
+     441      145720 :     uav_state = ret.value();
+     442             :   } else {
+     443           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Active estimator did not provide uav_state.", getName().c_str());
+     444           0 :     return;
+     445             :   }
+     446             : 
+     447      145720 :   if (!Support::noNans(uav_state.pose.orientation)) {
+     448           0 :     ROS_ERROR("[%s]: nan in uav state orientation", getName().c_str());
+     449           0 :     return;
+     450             :   }
+     451             : 
+     452             : 
+     453      145720 :   if (active_estimator_->isMitigatingJump()) {
+     454           0 :     estimator_switch_count_++;
+     455             :   }
+     456      145720 :   uav_state.estimator_iteration = estimator_switch_count_;
+     457             : 
+     458      145720 :   scope_timer.checkpoint("get uav state");
+     459             :   // TODO state health checks
+     460             : 
+     461      145720 :   ph_uav_state_.publish(uav_state);
+     462             : 
+     463      145720 :   scope_timer.checkpoint("pub uav state");
+     464      291440 :   nav_msgs::Odometry odom_main = Support::uavStateToOdom(uav_state);
+     465             : 
+     466      145720 :   scope_timer.checkpoint("uav state to odom");
+     467      291440 :   const std::vector<double> pose_covariance = active_estimator_->getPoseCovariance();
+     468     5391612 :   for (size_t i = 0; i < pose_covariance.size(); i++) {
+     469     5245886 :     odom_main.pose.covariance[i] = pose_covariance[i];
+     470             :   }
+     471             : 
+     472      291440 :   const std::vector<double> twist_covariance = active_estimator_->getTwistCovariance();
+     473     5391612 :   for (size_t i = 0; i < twist_covariance.size(); i++) {
+     474     5245886 :     odom_main.twist.covariance[i] = twist_covariance[i];
+     475             :   }
+     476             : 
+     477      145720 :   scope_timer.checkpoint("get covariance");
+     478      145720 :   ph_odom_main_.publish(odom_main);
+     479             : 
+     480      291440 :   nav_msgs::Odometry innovation = active_estimator_->getInnovation();
+     481      145720 :   ph_innovation_.publish(innovation);
+     482             : 
+     483             : 
+     484      291440 :   mrs_msgs::Float64Stamped agl_height;
+     485             : 
+     486      145720 :   if (is_using_agl_estimator_) {
+     487      116699 :     agl_height = est_alt_agl_->getUavAglHeight();
+     488      116699 :     ph_altitude_agl_.publish(agl_height);
+     489             :   }
+     490             : }
+     491             : /*//}*/
+     492             : 
+     493             : /*//{ timerPublishDiagnostics() */
+     494       16981 : void EstimationManager::timerPublishDiagnostics([[maybe_unused]] const ros::TimerEvent& event) {
+     495             : 
+     496       16981 :   if (!sm_->isInitialized()) {
+     497        1555 :     return;
+     498             :   }
+     499             : 
+     500       33962 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("EstimationManager::timerPublishDiagnostics", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     501             : 
+     502       16981 :   if (sm_->isInState(StateMachine::ESTIMATOR_SWITCHING_STATE)) {
+     503           0 :     ROS_WARN("[%s]: Not publishing diagnostics during estimator switching.", getName().c_str());
+     504           0 :     return;
+     505             :   }
+     506             : 
+     507       16981 :   if (!sm_->isInPublishableState()) {
+     508        1555 :     ROS_WARN_THROTTLE(1.0, "[%s]: not publishing uav state in %s", getName().c_str(), sm_->getCurrentStateString().c_str());
+     509        1555 :     return;
+     510             :   }
+     511             : 
+     512       15426 :   mrs_msgs::UavState uav_state;
+     513       15426 :   auto               ret = active_estimator_->getUavState();
+     514       15426 :   if (ret) {
+     515       15426 :     uav_state = ret.value();
+     516             :   } else {
+     517           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: Active estimator did not provide uav_state.", getName().c_str());
+     518           0 :     return;
+     519             :   }
+     520             : 
+     521       15426 :   if (!Support::noNans(uav_state.pose.orientation)) {
+     522           0 :     ROS_ERROR("[%s]: nan in uav state orientation", getName().c_str());
+     523           0 :     return;
+     524             :   }
+     525             : 
+     526       30852 :   mrs_msgs::Float64Stamped agl_height;
+     527             : 
+     528       15426 :   if (is_using_agl_estimator_) {
+     529       11422 :     agl_height = est_alt_agl_->getUavAglHeight();
+     530       11422 :     ph_altitude_agl_.publish(agl_height);
+     531             :   }
+     532             : 
+     533       30852 :   mrs_msgs::Float64Stamped max_flight_z_msg;
+     534       15426 :   max_flight_z_msg.header.stamp = ros::Time::now();
+     535       15426 :   if (active_estimator_ && active_estimator_->isInitialized()) {
+     536       15426 :     max_flight_z_msg.header.frame_id = active_estimator_->getFrameId();
+     537       15426 :     max_flight_z_msg.value           = active_estimator_->getMaxFlightZ();
+     538             :   }
+     539       15426 :   max_flight_z_ = max_flight_z_msg.value;
+     540       15426 :   ph_max_flight_z_.publish(max_flight_z_msg);
+     541             : 
+     542             :   // publish diagnostics
+     543       30852 :   mrs_msgs::EstimationDiagnostics diagnostics;
+     544             : 
+     545       15426 :   diagnostics.header.stamp   = uav_state.header.stamp;
+     546       15426 :   diagnostics.child_frame_id = uav_state.child_frame_id;
+     547             : 
+     548       15426 :   diagnostics.pose         = uav_state.pose;
+     549       15426 :   diagnostics.velocity     = uav_state.velocity;
+     550       15426 :   diagnostics.acceleration = uav_state.acceleration;
+     551             : 
+     552       15426 :   diagnostics.sm_state                 = sm_->getCurrentStateString();
+     553       15426 :   diagnostics.max_flight_z             = max_flight_z_msg.value;
+     554       15426 :   diagnostics.estimator_iteration      = estimator_switch_count_;
+     555       15426 :   diagnostics.estimation_rate          = ch_->desired_uav_state_rate;
+     556       15426 :   diagnostics.running_state_estimators = estimator_names_;
+     557             : 
+     558             :   {
+     559       30852 :     std::scoped_lock lock(mutex_switchable_estimator_names_);
+     560       15426 :     diagnostics.switchable_state_estimators = switchable_estimator_names_;
+     561             :   }
+     562             : 
+     563             : 
+     564       15426 :   if (active_estimator_ && active_estimator_->isInitialized()) {
+     565       15426 :     diagnostics.header.frame_id         = active_estimator_->getFrameId();
+     566       15426 :     diagnostics.current_state_estimator = active_estimator_->getName();
+     567             :   } else {
+     568           0 :     diagnostics.header.frame_id         = "";
+     569           0 :     diagnostics.current_state_estimator = "";
+     570             :   }
+     571             : 
+     572       15426 :   diagnostics.estimator_horizontal = uav_state.estimator_horizontal;
+     573       15426 :   diagnostics.estimator_vertical   = uav_state.estimator_vertical;
+     574       15426 :   diagnostics.estimator_heading    = uav_state.estimator_heading;
+     575             : 
+     576       15426 :   if (is_using_agl_estimator_) {
+     577       11422 :     diagnostics.estimator_agl_height = est_alt_agl_name_;
+     578       11422 :     diagnostics.agl_height           = agl_height.value;
+     579             :   } else {
+     580        4004 :     diagnostics.estimator_agl_height = "NOT_USED";
+     581        4004 :     diagnostics.agl_height           = std::nanf("");
+     582             :   }
+     583             : 
+     584       15426 :   diagnostics.platform_config = _platform_config_;
+     585       15426 :   diagnostics.custom_config   = _custom_config_;
+     586             : 
+     587       15426 :   ph_diagnostics_.publish(diagnostics);
+     588             : 
+     589       15426 :   ROS_INFO_THROTTLE(5.0, "[%s]: %s. pos: [%.2f, %.2f, %.2f] m. Estimator: %s. Max. z.: %.2f m. Estimator switches: %d.", getName().c_str(),
+     590             :                     sm_->getCurrentStateString().c_str(), uav_state.pose.position.x, uav_state.pose.position.y, uav_state.pose.position.z,
+     591             :                     active_estimator_->getName().c_str(), max_flight_z_, estimator_switch_count_);
+     592             : }
+     593             : /*//}*/
+     594             : 
+     595             : /*//{ timerCheckHealth() */
+     596      161740 : void EstimationManager::timerCheckHealth([[maybe_unused]] const ros::TimerEvent& event) {
+     597             : 
+     598      161740 :   if (!sm_->isInitialized()) {
+     599           5 :     return;
+     600             :   }
+     601             : 
+     602      485205 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("EstimationManager::timerCheckHealth", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     603             : 
+     604             :   /*//{ start ready estimators, check switchable estimators */
+     605      323470 :   std::vector<std::string> switchable_estimator_names;
+     606      338632 :   for (auto estimator : estimator_list_) {
+     607             : 
+     608      176897 :     if (estimator->isReady()) {
+     609             :       try {
+     610        6847 :         ROS_INFO_THROTTLE(1.0, "[%s]: starting the estimator '%s'", getName().c_str(), estimator->getName().c_str());
+     611        6847 :         estimator->start();
+     612             :       }
+     613           0 :       catch (std::runtime_error& ex) {
+     614           0 :         ROS_ERROR("[%s]: exception caught during estimator starting: '%s'", getName().c_str(), ex.what());
+     615           0 :         ros::shutdown();
+     616             :       }
+     617             :     }
+     618             : 
+     619      176897 :     if (estimator->isRunning() && estimator->getName() != "dummy" && estimator->getName() != "ground_truth") {
+     620      167877 :       switchable_estimator_names.push_back(estimator->getName());
+     621             :     }
+     622             :   }
+     623             : 
+     624             :   {
+     625      323470 :     std::scoped_lock lock(mutex_switchable_estimator_names_);
+     626      161735 :     switchable_estimator_names_ = switchable_estimator_names;
+     627             :   }
+     628             : 
+     629      161735 :   if (is_using_agl_estimator_ && est_alt_agl_->isReady()) {
+     630          61 :     est_alt_agl_->start();
+     631             :   }
+     632             : 
+     633             :   /*//}*/
+     634             : 
+     635      292268 :   if (!callbacks_disabled_by_service_ &&
+     636      292268 :       (sm_->isInState(StateMachine::FLYING_STATE) || sm_->isInState(StateMachine::HOVER_STATE) || sm_->isInState(StateMachine::READY_FOR_FLIGHT_STATE))) {
+     637      114362 :     callbacks_enabled_ = true;
+     638             :   } else {
+     639       47373 :     callbacks_enabled_ = false;
+     640             :   }
+     641             : 
+     642             :   // TODO fuj if, zmenit na switch
+     643             :   // activate initial estimator
+     644      161735 :   if (sm_->isInState(StateMachine::INITIALIZED_STATE) && initial_estimator_->isRunning()) {
+     645       14262 :     std::scoped_lock lock(mutex_active_estimator_);
+     646        7131 :     ROS_INFO_THROTTLE(1.0, "[%s]: activating the initial estimator %s", getName().c_str(), initial_estimator_->getName().c_str());
+     647        7131 :     active_estimator_ = initial_estimator_;
+     648        7131 :     if (active_estimator_->getName() == "dummy") {
+     649           0 :       sm_->changeState(StateMachine::DUMMY_STATE);
+     650             :     } else {
+     651        7131 :       if (!is_using_agl_estimator_ || est_alt_agl_->isRunning()) {
+     652          82 :         sm_->changeState(StateMachine::READY_FOR_FLIGHT_STATE);
+     653             :       } else {
+     654        7049 :         ROS_INFO_THROTTLE(1.0, "[%s]: %s agl estimator: %s to be running", getName().c_str(), Support::waiting_for_string.c_str(),
+     655             :                           est_alt_agl_->getName().c_str());
+     656             :       }
+     657             :     }
+     658             :   }
+     659             : 
+     660             :   // active estimator is in faulty state, we need to switch to healthy estimator
+     661      161735 :   if (sm_->isInTheAir() && active_estimator_->isError()) {
+     662           0 :     sm_->changeState(StateMachine::ESTIMATOR_SWITCHING_STATE);
+     663             :   }
+     664             : 
+     665      161735 :   if (sm_->isInState(StateMachine::ESTIMATOR_SWITCHING_STATE)) {
+     666           0 :     if (switchToHealthyEstimator()) {
+     667           0 :       sm_->changeToPreSwitchState();
+     668             :     } else {  // cannot switch to healthy estimator - failsafe necessary
+     669           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: Cannot switch to any healthy estimator. Triggering failsafe.", getName().c_str());
+     670           0 :       sm_->changeState(StateMachine::FAILSAFE_STATE);
+     671             :     }
+     672             :   }
+     673             : 
+     674      161735 :   if (sm_->isInState(StateMachine::FAILSAFE_STATE)) {
+     675           0 :     if (!failsafe_call_succeeded_ && callFailsafeService()) {
+     676           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: failsafe called successfully", getName().c_str());
+     677           0 :       failsafe_call_succeeded_ = true;
+     678             :     }
+     679           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: we are in failsafe state", getName().c_str());
+     680             :   }
+     681             : 
+     682             :   // standard takeoff
+     683      161735 :   if (sm_->isInState(StateMachine::READY_FOR_FLIGHT_STATE)) {
+     684       39266 :     if (sh_control_manager_diag_.hasMsg() && sh_control_manager_diag_.getMsg()->active_tracker == takeoff_tracker_name_) {
+     685          19 :       sm_->changeState(StateMachine::TAKING_OFF_STATE);
+     686             :     }
+     687             :   }
+     688             : 
+     689             :   // midair activation
+     690      161735 :   if (sm_->isInState(StateMachine::READY_FOR_FLIGHT_STATE)) {
+     691       39247 :     if (sh_control_manager_diag_.hasMsg() && sh_control_manager_diag_.getMsg()->active_tracker == after_midair_activation_tracker_name_) {
+     692          55 :       sm_->changeState(StateMachine::FLYING_STATE);
+     693             :     }
+     694             :   }
+     695             : 
+     696      161735 :   if (sm_->isInState(StateMachine::TAKING_OFF_STATE)) {
+     697        9819 :     if (sh_control_manager_diag_.hasMsg() && sh_control_manager_diag_.getMsg()->active_tracker != takeoff_tracker_name_) {
+     698          19 :       sm_->changeState(StateMachine::FLYING_STATE);
+     699             :     }
+     700             :   }
+     701             : 
+     702      161735 :   if (sm_->isInState(StateMachine::FLYING_STATE)) {
+     703       96793 :     if ((ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     704       18390 :       ROS_WARN_THROTTLE(1.0, "[%s]: not received control input for %.4fs, estimation suboptimal, potentially unstable", getName().c_str(),
+     705             :                         (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     706             :     }
+     707             :   }
+     708             : }
+     709             : /*//}*/
+     710             : 
+     711             : /*//{ timerInitialization() */
+     712          82 : void EstimationManager::timerInitialization([[maybe_unused]] const ros::TimerEvent& event) {
+     713             : 
+     714         164 :   mrs_lib::ParamLoader param_loader(nh_, getName());
+     715             : 
+     716          82 :   param_loader.loadParam("custom_config", _custom_config_);
+     717          82 :   param_loader.loadParam("platform_config", _platform_config_);
+     718          82 :   param_loader.loadParam("world_config", _world_config_);
+     719             : 
+     720          82 :   if (_custom_config_ != "") {
+     721          82 :     param_loader.addYamlFile(_custom_config_);
+     722             :   }
+     723             : 
+     724          82 :   if (_platform_config_ != "") {
+     725          82 :     param_loader.addYamlFile(_platform_config_);
+     726             :   }
+     727             : 
+     728          82 :   if (_world_config_ != "") {
+     729          82 :     param_loader.addYamlFile(_world_config_);
+     730             :   }
+     731             : 
+     732          82 :   param_loader.addYamlFileFromParam("private_config");
+     733          82 :   param_loader.addYamlFileFromParam("public_config");
+     734          82 :   param_loader.addYamlFileFromParam("uav_manager_config");
+     735          82 :   param_loader.addYamlFileFromParam("estimators_config");
+     736          82 :   param_loader.addYamlFileFromParam("active_estimators_config");
+     737             : 
+     738          82 :   param_loader.loadParam("uav_name", ch_->uav_name);
+     739             : 
+     740             :   /*//{ load world_origin parameters */
+     741             : 
+     742         164 :   std::string world_origin_units;
+     743          82 :   bool        is_origin_param_ok = true;
+     744          82 :   double      world_origin_x     = 0;
+     745          82 :   double      world_origin_y     = 0;
+     746             : 
+     747          82 :   param_loader.loadParam("world_origin/units", world_origin_units);
+     748             : 
+     749          82 :   if (Support::toLowercase(world_origin_units) == "utm") {
+     750           0 :     ROS_INFO("[%s]: Loading world origin in UTM units.", getName().c_str());
+     751           0 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_x", world_origin_x);
+     752           0 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_y", world_origin_y);
+     753             : 
+     754          82 :   } else if (Support::toLowercase(world_origin_units) == "latlon") {
+     755             :     double lat, lon;
+     756          82 :     ROS_INFO("[%s]: Loading world origin in LatLon units.", getName().c_str());
+     757          82 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_x", lat);
+     758          82 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_y", lon);
+     759          82 :     mrs_lib::UTM(lat, lon, &world_origin_x, &world_origin_y);
+     760          82 :     ROS_INFO("[%s]: Converted to UTM x: %f, y: %f.", getName().c_str(), world_origin_x, world_origin_y);
+     761             : 
+     762             :   } else {
+     763           0 :     ROS_ERROR("[%s]: world_origin_units must be (\"UTM\"|\"LATLON\"). Got '%s'", getName().c_str(), world_origin_units.c_str());
+     764           0 :     ros::shutdown();
+     765             :   }
+     766             : 
+     767          82 :   ch_->world_origin.x = world_origin_x;
+     768          82 :   ch_->world_origin.y = world_origin_y;
+     769             : 
+     770          82 :   if (!is_origin_param_ok) {
+     771           0 :     ROS_ERROR("[%s]: Could not load all mandatory parameters from world file. Please check your world file.", getName().c_str());
+     772           0 :     ros::shutdown();
+     773             :   }
+     774             :   /*//}*/
+     775             : 
+     776             :   /*//{ load tracker names */
+     777          82 :   param_loader.loadParam("mrs_uav_managers/uav_manager/takeoff/during_takeoff/tracker", takeoff_tracker_name_);
+     778          82 :   param_loader.loadParam("mrs_uav_managers/uav_manager/midair_activation/after_activation/tracker", after_midair_activation_tracker_name_);
+     779             :   /*//}*/
+     780             : 
+     781          82 :   param_loader.setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/");
+     782             : 
+     783             :   /*//{ load parameters into common handlers */
+     784          82 :   param_loader.loadParam("frame_id/fcu", ch_->frames.fcu);
+     785          82 :   ch_->frames.ns_fcu = ch_->uav_name + "/" + ch_->frames.fcu;
+     786             : 
+     787          82 :   param_loader.loadParam("frame_id/fcu_untilted", ch_->frames.fcu_untilted);
+     788          82 :   ch_->frames.ns_fcu_untilted = ch_->uav_name + "/" + ch_->frames.fcu_untilted;
+     789             : 
+     790          82 :   param_loader.loadParam("frame_id/rtk_antenna", ch_->frames.rtk_antenna);
+     791          82 :   ch_->frames.ns_rtk_antenna = ch_->uav_name + "/" + ch_->frames.rtk_antenna;
+     792             : 
+     793          82 :   ch_->transformer = std::make_shared<mrs_lib::Transformer>(nh_, getName());
+     794          82 :   ch_->transformer->retryLookupNewest(true);
+     795             : 
+     796          82 :   param_loader.loadParam("rate/diagnostics", ch_->desired_diagnostics_rate);
+     797             : 
+     798             :   /*//}*/
+     799             : 
+     800             :   /*//{ load parameters */
+     801             : 
+     802             :   /*//{ publish debug topics parameters */
+     803          82 :   param_loader.loadParam("debug_topics/input", ch_->debug_topics.input);
+     804          82 :   param_loader.loadParam("debug_topics/output", ch_->debug_topics.output);
+     805          82 :   param_loader.loadParam("debug_topics/state", ch_->debug_topics.state);
+     806          82 :   param_loader.loadParam("debug_topics/covariance", ch_->debug_topics.covariance);
+     807          82 :   param_loader.loadParam("debug_topics/innovation", ch_->debug_topics.innovation);
+     808          82 :   param_loader.loadParam("debug_topics/diagnostics", ch_->debug_topics.diag);
+     809          82 :   param_loader.loadParam("debug_topics/correction", ch_->debug_topics.correction);
+     810          82 :   param_loader.loadParam("debug_topics/correction_delay", ch_->debug_topics.corr_delay);
+     811             :   /*//}*/
+     812             : 
+     813             :   /*//}*/
+     814             : 
+     815         164 :   mrs_lib::SubscribeHandlerOptions shopts;
+     816          82 :   shopts.nh                 = nh_;
+     817          82 :   shopts.node_name          = getName();
+     818          82 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     819          82 :   shopts.threadsafe         = true;
+     820          82 :   shopts.autostart          = true;
+     821          82 :   shopts.queue_size         = 10;
+     822          82 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     823             : 
+     824             :   /*//{ wait for hw api message */
+     825             : 
+     826             :   mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities> sh_hw_api_capabilities_ =
+     827         246 :       mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+     828         164 :   while (!sh_hw_api_capabilities_.hasMsg()) {
+     829          82 :     ROS_INFO("[%s]: %s hw_api_capabilities message at topic: %s", getName().c_str(), Support::waiting_for_string.c_str(),
+     830             :              sh_hw_api_capabilities_.topicName().c_str());
+     831          82 :     ros::Duration(1.0).sleep();
+     832             :   }
+     833             : 
+     834         164 :   mrs_msgs::HwApiCapabilitiesConstPtr hw_api_capabilities = sh_hw_api_capabilities_.getMsg();
+     835             :   /*//}*/
+     836             : 
+     837             :   /*//{ wait for desired uav_state rate */
+     838          82 :   sh_control_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     839         179 :   while (!sh_control_manager_diag_.hasMsg()) {
+     840          97 :     ROS_INFO("[%s]: %s control_manager_diagnostics message at topic: %s", getName().c_str(), Support::waiting_for_string.c_str(),
+     841             :              sh_control_manager_diag_.topicName().c_str());
+     842          97 :     ros::Duration(1.0).sleep();
+     843             :   }
+     844             : 
+     845         164 :   mrs_msgs::ControlManagerDiagnosticsConstPtr control_manager_diag_msg = sh_control_manager_diag_.getMsg();
+     846          82 :   ch_->desired_uav_state_rate                                          = control_manager_diag_msg->desired_uav_state_rate;
+     847          82 :   ROS_INFO("[%s]: The estimation is running at: %.2f Hz", getName().c_str(), ch_->desired_uav_state_rate);
+     848             :   /*//}*/
+     849             : 
+     850             :   /*//{ initialize subscribers */
+     851             : 
+     852          82 :   sh_control_input_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>(shopts, "control_input_in");
+     853             : 
+     854             :   /*//}*/
+     855             : 
+     856             :   /*//{ load state estimator plugins */
+     857          82 :   param_loader.loadParam("state_estimators", estimator_names_);
+     858             : 
+     859          82 :   state_estimator_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::StateEstimator>>("mrs_uav_managers", "mrs_uav_managers::StateEstimator");
+     860             : 
+     861         168 :   for (size_t i = 0; i < estimator_names_.size(); i++) {
+     862             : 
+     863         172 :     const std::string estimator_name = estimator_names_[i];
+     864             : 
+     865             :     // load the estimator parameters
+     866         172 :     std::string address;
+     867          86 :     param_loader.loadParam(estimator_name + "/address", address);
+     868             : 
+     869             :     try {
+     870          86 :       ROS_INFO("[%s]: loading the estimator '%s'", getName().c_str(), address.c_str());
+     871          86 :       estimator_list_.push_back(state_estimator_loader_->createInstance(address.c_str()));
+     872             :     }
+     873           0 :     catch (pluginlib::CreateClassException& ex1) {
+     874           0 :       ROS_ERROR("[%s]: CreateClassException for the estimator '%s'", getName().c_str(), address.c_str());
+     875           0 :       ROS_ERROR("[%s]: Error: %s", getName().c_str(), ex1.what());
+     876           0 :       ros::shutdown();
+     877             :     }
+     878           0 :     catch (pluginlib::PluginlibException& ex) {
+     879           0 :       ROS_ERROR("[%s]: PluginlibException for the estimator '%s'", getName().c_str(), address.c_str());
+     880           0 :       ROS_ERROR("[%s]: Error: %s", getName().c_str(), ex.what());
+     881           0 :       ros::shutdown();
+     882             :     }
+     883             :   }
+     884             : 
+     885             :   /*//{ load agl estimator plugins */
+     886          82 :   param_loader.loadParam("agl_height_estimator", est_alt_agl_name_);
+     887          82 :   is_using_agl_estimator_ = est_alt_agl_name_ != "";
+     888          82 :   ROS_WARN_COND(!is_using_agl_estimator_, "[%s]: not using AGL estimator for min height safe checking", getName().c_str());
+     889             : 
+     890             : 
+     891          82 :   if (is_using_agl_estimator_) {
+     892             : 
+     893          61 :     agl_estimator_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::AglEstimator>>("mrs_uav_managers", "mrs_uav_managers::AglEstimator");
+     894             : 
+     895             :     // load the estimator parameters
+     896         122 :     std::string address;
+     897          61 :     param_loader.loadParam(est_alt_agl_name_ + "/address", address);
+     898             : 
+     899             :     try {
+     900          61 :       ROS_INFO("[%s]: loading the estimator '%s'", getName().c_str(), address.c_str());
+     901          61 :       est_alt_agl_ = agl_estimator_loader_->createInstance(address.c_str());
+     902             :     }
+     903           0 :     catch (pluginlib::CreateClassException& ex1) {
+     904           0 :       ROS_ERROR("[%s]: CreateClassException for the estimator '%s'", getName().c_str(), address.c_str());
+     905           0 :       ROS_ERROR("[%s]: Error: %s", getName().c_str(), ex1.what());
+     906           0 :       ros::shutdown();
+     907             :     }
+     908           0 :     catch (pluginlib::PluginlibException& ex) {
+     909           0 :       ROS_ERROR("[%s]: PluginlibException for the estimator '%s'", getName().c_str(), address.c_str());
+     910           0 :       ROS_ERROR("[%s]: Error: %s", getName().c_str(), ex.what());
+     911           0 :       ros::shutdown();
+     912             :     }
+     913             :   }
+     914             :   /*//}*/
+     915             : 
+     916          82 :   ROS_INFO("[%s]: estimators were loaded", getName().c_str());
+     917             :   /*//}*/
+     918             : 
+     919             :   /*//{ check whether initial estimator was loaded */
+     920          82 :   param_loader.loadParam("initial_state_estimator", initial_estimator_name_);
+     921          82 :   bool initial_estimator_found = false;
+     922          82 :   for (auto estimator : estimator_list_) {
+     923          82 :     if (estimator->getName() == initial_estimator_name_) {
+     924          82 :       initial_estimator_      = estimator;
+     925          82 :       initial_estimator_found = true;
+     926          82 :       break;
+     927             :     }
+     928             :   }
+     929             : 
+     930          82 :   if (!initial_estimator_found) {
+     931           0 :     ROS_ERROR("[%s]: initial estimator %s could not be found among loaded estimators. shutting down", getName().c_str(), initial_estimator_name_.c_str());
+     932           0 :     ros::shutdown();
+     933             :   }
+     934             :   /*//}*/
+     935             : 
+     936             :   /*//{ initialize estimators */
+     937         168 :   for (auto estimator : estimator_list_) {
+     938             : 
+     939             :     // create private handlers
+     940         172 :     std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> ph = std::make_shared<mrs_uav_managers::estimation_manager::PrivateHandlers_t>();
+     941             : 
+     942          86 :     ph->loadConfigFile = boost::bind(&EstimationManager::loadConfigFile, this, _1);
+     943          86 :     ph->param_loader   = std::make_shared<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, estimator->getName()), "EstimationManager/" + estimator->getName());
+     944             : 
+     945          86 :     if (_custom_config_ != "") {
+     946          86 :       ph->param_loader->addYamlFile(_custom_config_);
+     947             :     }
+     948             : 
+     949          86 :     if (_platform_config_ != "") {
+     950          86 :       ph->param_loader->addYamlFile(_platform_config_);
+     951             :     }
+     952             : 
+     953          86 :     if (_world_config_ != "") {
+     954          86 :       ph->param_loader->addYamlFile(_world_config_);
+     955             :     }
+     956             : 
+     957             :     try {
+     958          86 :       ROS_INFO("[%s]: initializing the estimator '%s'", getName().c_str(), estimator->getName().c_str());
+     959          86 :       estimator->initialize(nh_, ch_, ph);
+     960             :     }
+     961           0 :     catch (std::runtime_error& ex) {
+     962           0 :       ROS_ERROR("[%s]: exception caught during estimator initialization: '%s'", getName().c_str(), ex.what());
+     963           0 :       ros::shutdown();
+     964             :     }
+     965             : 
+     966          86 :     if (!estimator->isCompatibleWithHwApi(hw_api_capabilities)) {
+     967           0 :       ROS_ERROR("[%s]: estimator %s is not compatible with the hw api. Shutting down.", getName().c_str(), estimator->getName().c_str());
+     968           0 :       ros::shutdown();
+     969             :     }
+     970             :   }
+     971             : 
+     972             :   // | ----------- agl height estimator initialization ---------- |
+     973          82 :   if (is_using_agl_estimator_) {
+     974             : 
+     975         122 :     std::shared_ptr<mrs_uav_managers::estimation_manager::PrivateHandlers_t> ph = std::make_shared<mrs_uav_managers::estimation_manager::PrivateHandlers_t>();
+     976             : 
+     977          61 :     ph->loadConfigFile = boost::bind(&EstimationManager::loadConfigFile, this, _1);
+     978          61 :     ph->param_loader   = std::make_shared<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, est_alt_agl_->getName()), "EstimationManager/" + est_alt_agl_->getName());
+     979             : 
+     980          61 :     if (_custom_config_ != "") {
+     981          61 :       ph->param_loader->addYamlFile(_custom_config_);
+     982             :     }
+     983             : 
+     984          61 :     if (_platform_config_ != "") {
+     985          61 :       ph->param_loader->addYamlFile(_platform_config_);
+     986             :     }
+     987             : 
+     988          61 :     if (_world_config_ != "") {
+     989          61 :       ph->param_loader->addYamlFile(_world_config_);
+     990             :     }
+     991             : 
+     992             :     try {
+     993          61 :       ROS_INFO("[%s]: initializing the estimator '%s'", getName().c_str(), est_alt_agl_->getName().c_str());
+     994          61 :       est_alt_agl_->initialize(nh_, ch_, ph);
+     995             :     }
+     996           0 :     catch (std::runtime_error& ex) {
+     997           0 :       ROS_ERROR("[%s]: exception caught during estimator initialization: '%s'", getName().c_str(), ex.what());
+     998           0 :       ros::shutdown();
+     999             :     }
+    1000             : 
+    1001          61 :     if (!est_alt_agl_->isCompatibleWithHwApi(hw_api_capabilities)) {
+    1002           0 :       ROS_ERROR("[%s]: estimator %s is not compatible with the hw api. Shutting down.", getName().c_str(), est_alt_agl_->getName().c_str());
+    1003           0 :       ros::shutdown();
+    1004             :     }
+    1005             :   }
+    1006             : 
+    1007          82 :   ROS_INFO("[%s]: estimators were initialized", getName().c_str());
+    1008             : 
+    1009             :   /*//}*/
+    1010             : 
+    1011             :   /*//{ initialize publishers */
+    1012          82 :   ph_uav_state_    = mrs_lib::PublisherHandler<mrs_msgs::UavState>(nh_, "uav_state_out", 10);
+    1013          82 :   ph_odom_main_    = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "odom_main_out", 10);
+    1014          82 :   ph_innovation_   = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "innovation_out", 10);
+    1015          82 :   ph_diagnostics_  = mrs_lib::PublisherHandler<mrs_msgs::EstimationDiagnostics>(nh_, "diagnostics_out", 10);
+    1016          82 :   ph_max_flight_z_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "max_flight_z_agl_out", 10);
+    1017          82 :   ph_altitude_agl_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "height_agl_out", 10);
+    1018             : 
+    1019             :   /*//}*/
+    1020             : 
+    1021             :   /*//{ initialize timers */
+    1022          82 :   timer_publish_ = nh_.createTimer(ros::Rate(ch_->desired_uav_state_rate), &EstimationManager::timerPublish, this);
+    1023             : 
+    1024          82 :   timer_publish_diagnostics_ = nh_.createTimer(ros::Rate(ch_->desired_diagnostics_rate), &EstimationManager::timerPublishDiagnostics, this);
+    1025             : 
+    1026          82 :   timer_check_health_ = nh_.createTimer(ros::Rate(ch_->desired_uav_state_rate), &EstimationManager::timerCheckHealth, this);
+    1027             :   /*//}*/
+    1028             : 
+    1029             :   /*//{ initialize service clients */
+    1030             : 
+    1031          82 :   srvch_failsafe_.initialize(nh_, "failsafe_out");
+    1032             : 
+    1033             :   /*//}*/
+    1034             : 
+    1035             :   /*//{ initialize service servers */
+    1036          82 :   srvs_change_estimator_ = nh_.advertiseService("change_estimator_in", &EstimationManager::callbackChangeEstimator, this);
+    1037          82 :   srvs_toggle_callbacks_ = nh_.advertiseService("toggle_service_callbacks_in", &EstimationManager::callbackToggleServiceCallbacks, this);
+    1038             :   /*//}*/
+    1039             : 
+    1040             :   /*//{ initialize scope timer */
+    1041          82 :   param_loader.loadParam("scope_timer/enabled", ch_->scope_timer.enabled);
+    1042         164 :   std::string       filepath;
+    1043         164 :   const std::string time_logger_filepath = ros::package::getPath(package_name_) + "/scope_timer/scope_timer.txt";
+    1044          82 :   ch_->scope_timer.logger                = std::make_shared<mrs_lib::ScopeTimerLogger>(time_logger_filepath, ch_->scope_timer.enabled);
+    1045             :   /*//}*/
+    1046             : 
+    1047          82 :   if (!param_loader.loadedSuccessfully()) {
+    1048           0 :     ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getName().c_str());
+    1049           0 :     ros::shutdown();
+    1050             :   }
+    1051             : 
+    1052          82 :   sm_->changeState(StateMachine::INITIALIZED_STATE);
+    1053             : 
+    1054          82 :   ROS_INFO("[%s]: initialized", getName().c_str());
+    1055          82 : }
+    1056             : /*//}*/
+    1057             : 
+    1058             : // | -------------------- service callbacks ------------------- |
+    1059             : 
+    1060             : /*//{ callbackChangeEstimator() */
+    1061           4 : bool EstimationManager::callbackChangeEstimator(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+    1062             : 
+    1063           4 :   if (!sm_->isInitialized()) {
+    1064           0 :     return false;
+    1065             :   }
+    1066             : 
+    1067           4 :   if (!callbacks_enabled_) {
+    1068           0 :     res.success = false;
+    1069           0 :     res.message = ("Service callbacks are disabled");
+    1070           0 :     ROS_WARN("[%s]: Ignoring service call. Callbacks are disabled.", getName().c_str());
+    1071           0 :     return true;
+    1072             :   }
+    1073             : 
+    1074           4 :   if (req.value == "dummy" || req.value == "ground_truth") {
+    1075           0 :     res.success = false;
+    1076           0 :     std::stringstream ss;
+    1077           0 :     ss << "Switching to " << req.value << " estimator is not allowed.";
+    1078           0 :     res.message = ss.str();
+    1079           0 :     ROS_WARN("[%s]: Switching to %s estimator is not allowed.", getName().c_str(), req.value.c_str());
+    1080           0 :     return true;
+    1081             :   }
+    1082             : 
+    1083             :   // we do not want the switch to be disturbed by a service call
+    1084           4 :   callbacks_enabled_ = false;
+    1085             : 
+    1086           4 :   bool                                                target_estimator_found = false;
+    1087           4 :   boost::shared_ptr<mrs_uav_managers::StateEstimator> target_estimator;
+    1088          10 :   for (auto estimator : estimator_list_) {
+    1089          10 :     if (estimator->getName() == req.value) {
+    1090           4 :       target_estimator       = estimator;
+    1091           4 :       target_estimator_found = true;
+    1092           4 :       break;
+    1093             :     }
+    1094             :   }
+    1095             : 
+    1096           4 :   if (target_estimator_found) {
+    1097             : 
+    1098           4 :     if (target_estimator->isRunning()) {
+    1099           4 :       sm_->changeState(StateMachine::ESTIMATOR_SWITCHING_STATE);
+    1100           4 :       switchToEstimator(target_estimator);
+    1101           4 :       sm_->changeToPreSwitchState();
+    1102             :     } else {
+    1103           0 :       ROS_WARN("[%s]: Switch to not running estimator %s requested", getName().c_str(), req.value.c_str());
+    1104           0 :       res.success = false;
+    1105           0 :       res.message = ("Requested estimator is not running");
+    1106           0 :       return true;
+    1107             :     }
+    1108             : 
+    1109             :   } else {
+    1110           0 :     ROS_WARN("[%s]: Switch to invalid estimator %s requested", getName().c_str(), req.value.c_str());
+    1111           0 :     res.success = false;
+    1112           0 :     res.message = ("Not a valid estimator type");
+    1113           0 :     return true;
+    1114             :   }
+    1115             : 
+    1116           4 :   res.success = true;
+    1117           4 :   res.message = "Estimator switch successful";
+    1118             : 
+    1119             :   // allow service calllbacks after switch again
+    1120           4 :   callbacks_enabled_ = true;
+    1121             : 
+    1122           4 :   return true;
+    1123             : }
+    1124             : /*//}*/
+    1125             : 
+    1126             : /* //{ callbackToggleServiceCallbacks() */
+    1127         113 : bool EstimationManager::callbackToggleServiceCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    1128             : 
+    1129         113 :   if (!sm_->isInitialized()) {
+    1130           0 :     ROS_ERROR("[%s]: service for toggling callbacks is not available before initialization.", getName().c_str());
+    1131           0 :     return false;
+    1132             :   }
+    1133             : 
+    1134         113 :   callbacks_disabled_by_service_ = !req.data;
+    1135             : 
+    1136         113 :   res.success = true;
+    1137         113 :   res.message = (callbacks_disabled_by_service_ ? "Service callbacks disabled" : "Service callbacks enabled");
+    1138             : 
+    1139         113 :   if (callbacks_disabled_by_service_) {
+    1140             : 
+    1141          39 :     ROS_INFO("[%s]: Service callbacks disabled.", getName().c_str());
+    1142             : 
+    1143             :   } else {
+    1144             : 
+    1145          74 :     ROS_INFO("[%s]: Service callbacks enabled", getName().c_str());
+    1146             :   }
+    1147             : 
+    1148         113 :   return true;
+    1149             : }
+    1150             : 
+    1151             : //}
+    1152             : 
+    1153             : 
+    1154             : // --------------------------------------------------------------
+    1155             : // |                        other methods                       |
+    1156             : // --------------------------------------------------------------
+    1157             : //
+    1158             : /*//{ switchToHealthyEstimator() */
+    1159           0 : bool EstimationManager::switchToHealthyEstimator() {
+    1160             : 
+    1161             :   // available estimators should be specified in decreasing priority order in config file
+    1162           0 :   for (auto estimator : estimator_list_) {
+    1163           0 :     if (estimator->isRunning()) {
+    1164           0 :       switchToEstimator(estimator);
+    1165           0 :       return true;
+    1166             :     }
+    1167             :   }
+    1168           0 :   return false;  // no other estimator is running
+    1169             : }
+    1170             : /*//}*/
+    1171             : 
+    1172             : /*//{ switchToEstimator() */
+    1173           4 : void EstimationManager::switchToEstimator(const boost::shared_ptr<mrs_uav_managers::StateEstimator>& target_estimator) {
+    1174             : 
+    1175           4 :   std::scoped_lock lock(mutex_active_estimator_);
+    1176           4 :   ROS_INFO("[%s]: switching estimator from %s to %s", getName().c_str(), active_estimator_->getName().c_str(), target_estimator->getName().c_str());
+    1177           4 :   active_estimator_ = target_estimator;
+    1178           4 :   estimator_switch_count_++;
+    1179           4 : }
+    1180             : /*//}*/
+    1181             : 
+    1182             : /*//{ callFailsafeService() */
+    1183           0 : bool EstimationManager::callFailsafeService() {
+    1184           0 :   std_srvs::Trigger srv_out;
+    1185           0 :   return srvch_failsafe_.call(srv_out);
+    1186             : }
+    1187             : /*//}*/
+    1188             : 
+    1189             : /*//{ getName() */
+    1190        2779 : std::string EstimationManager::getName() const {
+    1191        2779 :   return nodelet_name_;
+    1192             : }
+    1193             : /*//}*/
+    1194             : 
+    1195             : /* loadConfigFile() //{ */
+    1196             : 
+    1197           0 : bool EstimationManager::loadConfigFile(const std::string& file_path) {
+    1198             : 
+    1199           0 :   const std::string name_space = nh_.getNamespace() + "/";
+    1200             : 
+    1201           0 :   ROS_INFO("[%s]: loading '%s' under the namespace '%s'", getName().c_str(), file_path.c_str(), name_space.c_str());
+    1202             : 
+    1203             :   // load the user-requested file
+    1204             :   {
+    1205           0 :     std::string command = "rosparam load " + file_path + " " + name_space;
+    1206           0 :     int         result  = std::system(command.c_str());
+    1207             : 
+    1208           0 :     if (result != 0) {
+    1209           0 :       ROS_ERROR("[%s]: failed to load '%s'", getName().c_str(), file_path.c_str());
+    1210           0 :       return false;
+    1211             :     }
+    1212             :   }
+    1213             : 
+    1214             :   // load the platform config
+    1215           0 :   if (_platform_config_ != "") {
+    1216           0 :     std::string command = "rosparam load " + _platform_config_ + " " + name_space;
+    1217           0 :     int         result  = std::system(command.c_str());
+    1218             : 
+    1219           0 :     if (result != 0) {
+    1220           0 :       ROS_ERROR("[%s]: failed to load the platform config file '%s'", getName().c_str(), _platform_config_.c_str());
+    1221           0 :       return false;
+    1222             :     }
+    1223             :   }
+    1224             : 
+    1225             :   // load the custom config
+    1226           0 :   if (_custom_config_ != "") {
+    1227           0 :     std::string command = "rosparam load " + _custom_config_ + " " + name_space;
+    1228           0 :     int         result  = std::system(command.c_str());
+    1229             : 
+    1230           0 :     if (result != 0) {
+    1231           0 :       ROS_ERROR("[%s]: failed to load the custom config file '%s'", getName().c_str(), _custom_config_.c_str());
+    1232           0 :       return false;
+    1233             :     }
+    1234             :   }
+    1235             : 
+    1236           0 :   return true;
+    1237             : }
+    1238             : 
+    1239             : //}
+    1240             : 
+    1241             : }  // namespace estimation_manager
+    1242             : 
+    1243             : }  // namespace mrs_uav_managers
+    1244             : 
+    1245             : #include <pluginlib/class_list_macros.h>
+    1246          82 : 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..4af9db1b17 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimation_manager.cpp.gcov.overview.html @@ -0,0 +1,332 @@ + + + + + + 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 + + +
+ 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..7c075a1435facc1fbba70d6050928450ee90fae5 GIT binary patch literal 4322 zcmV<85FPJ{P)YvK+c4e= z?SR_<+|7UnU{k{w-%TPG0Twwr)wRSF2Y8JFg7FeA4x>Vy8SrG+3NyJJO@Q{UWu6#7 zo^@U+uE>dE6j3oX#tzdsT85=@ zzqD(u?+#sS(nbOw(Dw#PC;&8Kw9~i?$IC}uv=dZyA392tt6*fzjkN)={b1ji8CL+b zV=VA*9)7cy*a%ErTLH~o8}QcBSOzuEqLY(NZljeKb8Ads1;XhO+I;+7+>2#-z_u*6 zX_-vzW&_&+L!bU}aZhFj<0FAalH%k0lf?sCX*4%XV@6sHjjr{j(e=6TT^uwn@*qbB z5Hm9b3NJiEZmehav-rmTx+m8%!%euRY|Rx@!QSq7;P~u|ytWC*wwj#HY`$ntNTp@T z>r)}U(P_GpzIe-$(q?&0~DSJ@xhm+zKEb(5^i|DGDZ~Mm&kY6667BQ8dSl zXh<;ECZlpz;JN%t0*^~}c+y(nua!{DZH>Ja{^F5Q{YPOckonVmWVYWca9P~4z~i&< zq^-dBcMVg)yK}#4WS(g)6xYGXOk6Zq??NzNX*n_*#@J5a9-#@)ZIZ4On6LhG@Uc7NIiduz@KoZsYnv;G`A>A$h?34bClxeFr(U(pTmr98Z&0#ButF$Pv6t= zvjL6PLDJ+=M88XN6>M=u=fo9#6a+Xe3S33k^;`!}(fwZz*AY0NK5j-s!#S~)UC(9b ztRjxvJQsI^H4S$qhfdseF>nUdOjC&(t|u7@5s#f|Yl<|8=SX{KSzQ3wC8KC=SWP9zZ{!di{dB8KDBA?quv zgg)7|MgZ>mj=ndZkYd)gjDU81oo~vuQuB2qTwmiEXiw<~MtoOHQ;s{5H{i$s4&{3B z2CX48ei(8<&&NnGOBhEASiubcex@f9`2&KoVchAhp>c4EcP}ZQcm#qTVkn%INPF}W z#>htr5JTIz_9%z&AMVS+Q45uoLs;ZY6== zogUe^zH7{kIiQKWI%bBvvn$Pb#*YGull)}Y!HU%4APB3EN@&HQRZ&-Ci(H@Ok!L#v z0kz#;1;RdY^Rf>?%gv?2ubQ(d`7r@5(q47i1vCw?79)s~ve+ANO;BFN$Q~LnV~mqf zm1PB>wd=#D2=2o$1{Y{|1BNk1w%$Gk80(CE`1)ov6w{3r8cU;ja-BTynk^$e&$|u) z<=|}~F#jh{(B2JJB|pIe1~65NK57)#P@z4Al!jrnyPcm&YATt7vs`B_is1%oAGT2K z5YLpP;)j<5G?X>mjORmYDk>>6+DW^_<1Wp`Kb;>iEB+m0?G6_yLl`Wl%dg=^$6718 zj(`(XE|qH&LZCgM*klXZv?VS2D(w-p2r#qmr7~NjK2^PKj>gY1bCK(OQ1K8dbv|}x zCMsrLZ%;dMepd58!oBLSYoEb)(A~Vz)X3Rjz^k)WBc04c`dn)csnMPgYUCT9b#;(ta zXU+!X6xT>m#APukMW$Dj_q>cu5RhGa%no_CU_lBENr19v9|sFuZcslmf1W)S1MAsy z{{2_^W`CajpJ#twdju`|5uW}3bLF}s9#%r~?js8!gS!$KMM{P==bD!k20XsY`|Ne~ z;SaKE@B+{J;Xx~-u9h56Rum8TKi4(Fh-Ij8jfH)&>-|+ksMCmsS!YJ(<(zoJdsNSu zt{Fg{#>b&Icmu=(E=&U{UQYs#~? zW~W>QzkF`YDoOzlU>>mV>Uv0y3Ny%&zRgZ8W}V?DLeKS@PFJTP(o;hU0mT*DY0ymHh4d6Kt6q74VNx%EIL>1JG zp%ztu8N{`h4coEvfI%i|59vfSv#<+Y_qndM?I`9Eq=n^4)JN)+L-EYo0r^ud zmw3$9V>FIA-5oR6{GN+~!U?x74z9V9t=jU_41nYBpk9BxB+xQsBu2<8=k z@f7XT*(o|&;1*WWa*MkeF7I%cW06e&HUV>@Q8kbYV8*6s>{>Vn|8id9l8D!9v16xZ ztuF-W^ZOIN9Cz>sUdKc0{gY4utZl@hbh%9~s~Z0@9!|)-fh(QDHk|J|*KmPGui?=- zi5Y@zi+&1&5hNB?m{xE%?g}E@vaXGC>~LLzJpq9PzQLD3_=M|_b6JzC2reUn2v_Om zX1jRuf!SX1;x6E-_NZz~%g0H96QMIUoDZOBjwUT&`JAJf3ZTD6Joz&GG4UuqMOK6K z`?+SWsfU+>b6=T{YAwQbO0s)TY7X8N@&Giyq&8?VI01RA*{a_5-6 zEFlGu<`ra&Bm=3yEoO`lm`*h|1|y$wu8re{3yBN|Zj2+N?wBJp`(t872!GuCt!~GfyvLm}BPY!x6+D1I6(kT@3L}k@z_G!~3#!s@<>CF*a=mL> z948dd(V0UuOxuVW;&GwtnTkDMkW6>-i)Ze(8$7sdKADIM^@EF6kMfT2!NJe)?mE&_Ut z+BOA{C7oeAv*#Mda1q7jmRDw_awzb;SSAss1U>_qMu-tla)xJw#Cz=li*~6zt?*^# z%XIw@q6%Qk0p!tVUOT0@$NMu3rde1x1C7jnhdI&B4&ed&K*OjHp;+_r=^=ESQK50A z>9k?Mc!X>TFJ^#u?xz%S5;wS|QH{TY_7#3lyhlc0yx_#4q6Ql?5Thocs+*M#T!u5^ zR*Z3C2Lb_NJPVB&W8KcZkT#q^aIZ)sZ3`g!#G-jK-Xk=T5YiH!;^0iVWDtzCF=<8IBiPyXieLme&@>aq%8>x!?6o`mky%H}5G>T@>T@?8BYfl8s=_D7&nDd4DA z1KNDyxhAiWXGwB=?|%T0yZ#`xQwP6FMK)sMg#udVjI~Y1N}wJErzdDsD}fXmGr-m~ zp7f+1%&EOcJgud*^W&6%R5WqS97~C z)1#!?$Kb#L5D;LGI@ouIB)31)Tx3 z>gQ7$rY(RcS+=*nwy+XVv)~wTQB6I}oT-3I4d8c-jGBp`EWBjrT{UL-f+|kn^%&Lk z-qJf&sl~(1jjmhLbvxgpJ?1QcR_$g1JU;3?glZ*eWzpA8Wq1^5%u<>>_X|#v_-}J! z>Qb4?5X9XFAQcyb8+E^^NSih~GR8)hVfg^aXMaow|0o&S$6frPn!CQT>xcY;%GVX{ zqTsa*c&OQ0zNfbL4FiUP{Nwb$P9p$J7YrYdEMPhj@59Eg;OeHVlzU#}x|?{GRA~ij zu;vTmabhU|`eP&OwuN1QmuJ`uK#cw~OVqRsQRrN>fiG%_1;*7y}` zkaR#BMv+F_g;F2}K(Afds3EmkDGUg3C98t~%ci13ET#TP9=XQ`$p(0PRZHBqMRd9G z{p`vW?2bNd0pJ4$^jO6;HJ8$iWzy*^i$J4ppz6AIr&!D8%u%Gc*ySKOr4uy%)3J7-c`Qn8KubLG@Izs#M;A^C&a`364R>~6(q zwz&2o~fe3Rx_eTQbhLa5^ms^ zapM)4{?a4jEqF`oe0!{xH-W3ct>mbU7w-($4(&(4=R zsp$e?b!11$}>aqvL zEl#g5P~7riQ%+51#+wUEoTxSRO3}p`4hqre2tBVoYtj>MsO_sltU&NzPj%!Q(z+^) z5#biI5HEBuXt*=gh7la8I94fnn9RohnMe#f+|}{EuKvl(aqTA4{*qMiqv=#39=B-j z%S;Inm-w&n0053?+V&|Qu3!s8IH>dN2{{xCVnCdN8A3FS-!_EcirhFMe%KJ|BM632 z9l_ENriu8CLr6yOXAGe+VpriRm|OdO;W&^4YIBU&X^a~#&{#muTW8wxAM^@duCC=v Q6aWAK07*qoM6N<$f(qFs_5c6? 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..0f55d28d99 --- /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:628572.9 %
Date:2024-04-09 22:03:33Functions: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)1852
mrs_uav_managers::Estimator::setCurrentSmState(mrs_uav_managers::estimation_manager::SMStates_t const&)1852
mrs_uav_managers::Estimator::getCurrentSmStateString[abi:cxx11]() const1852
mrs_uav_managers::Estimator::getPrintName[abi:cxx11]() const2775
mrs_uav_managers::Estimator::getSmStateString[abi:cxx11](mrs_uav_managers::estimation_manager::SMStates_t const&) const3704
mrs_uav_managers::Estimator::getMaxFlightZ() const15742
mrs_uav_managers::Estimator::isStarted() const32720
mrs_uav_managers::Estimator::getAccGlobal(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, double)247817
mrs_uav_managers::Estimator::getAccGlobal(boost::shared_ptr<mrs_msgs::EstimatorInput_<std::allocator<void> > const> const&, double)248458
mrs_uav_managers::Estimator::isReady() const308636
mrs_uav_managers::Estimator::getName[abi:cxx11]() const533883
mrs_uav_managers::Estimator::getFrameId[abi:cxx11]() const627138
mrs_uav_managers::Estimator::isMitigatingJump() const645822
mrs_uav_managers::Estimator::publishDiagnostics() const886248
mrs_uav_managers::Estimator::isError() const1125902
mrs_uav_managers::Estimator::isRunning() const1462559
mrs_uav_managers::Estimator::isInitialized() const2289751
mrs_uav_managers::Estimator::isInState(mrs_uav_managers::estimation_manager::SMStates_t const&) const6777018
mrs_uav_managers::Estimator::getCurrentSmState() const7441034
+
+
+ + + +
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..827beacddf --- /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:628572.9 %
Date:2024-04-09 22:03:33Functions: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)1852
mrs_uav_managers::Estimator::getAccGlobal(geometry_msgs::Vector3Stamped_<std::allocator<void> > const&, double)247817
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)248458
mrs_uav_managers::Estimator::getHeadingRate(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const> const&)0
mrs_uav_managers::Estimator::setCurrentSmState(mrs_uav_managers::estimation_manager::SMStates_t const&)1852
mrs_uav_managers::Estimator::getFrameId[abi:cxx11]() const627138
mrs_uav_managers::Estimator::getPrintName[abi:cxx11]() const2775
mrs_uav_managers::Estimator::getMaxFlightZ() const15742
mrs_uav_managers::Estimator::isInitialized() const2289751
mrs_uav_managers::Estimator::getSmStateString[abi:cxx11](mrs_uav_managers::estimation_manager::SMStates_t const&) const3704
mrs_uav_managers::Estimator::isMitigatingJump() const645822
mrs_uav_managers::Estimator::getCurrentSmState() const7441034
mrs_uav_managers::Estimator::publishDiagnostics() const886248
mrs_uav_managers::Estimator::getCurrentSmStateString[abi:cxx11]() const1852
mrs_uav_managers::Estimator::getName[abi:cxx11]() const533883
mrs_uav_managers::Estimator::getType[abi:cxx11]() const0
mrs_uav_managers::Estimator::isError() const1125902
mrs_uav_managers::Estimator::isReady() const308636
mrs_uav_managers::Estimator::isInState(mrs_uav_managers::estimation_manager::SMStates_t const&) const6777018
mrs_uav_managers::Estimator::isRunning() const1462559
mrs_uav_managers::Estimator::isStarted() const32720
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..6d516bdbc1 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.html @@ -0,0 +1,290 @@ + + + + + + + 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:628572.9 %
Date:2024-04-09 22:03:33Functions: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        1852 : bool Estimator::changeState(SMStates_t new_state) {
+       9             : 
+      10        1852 :   if (new_state == getCurrentSmState()) {
+      11           0 :     return true;
+      12             :   }
+      13             : 
+      14        1852 :   previous_sm_state_ = getCurrentSmState();
+      15        1852 :   setCurrentSmState(new_state);
+      16             : 
+      17        1852 :   ROS_INFO("[%s]: Switching sm state %s -> %s", getPrintName().c_str(), getSmStateString(previous_sm_state_).c_str(), getCurrentSmStateString().c_str());
+      18        1852 :   return true;
+      19             : }
+      20             : /*//}*/
+      21             : 
+      22             : /*//{ isInState() */
+      23     6777018 : bool Estimator::isInState(const SMStates_t& state_in) const {
+      24     6777018 :   return state_in == getCurrentSmState();
+      25             : }
+      26             : /*//}*/
+      27             : 
+      28             : /*//{ isInitialized() */
+      29     2289751 : bool Estimator::isInitialized() const {
+      30     2289751 :   return !isInState(UNINITIALIZED_STATE);
+      31             : }
+      32             : /*//}*/
+      33             : 
+      34             : /*//{ isReady() */
+      35      308636 : bool Estimator::isReady() const {
+      36      308636 :   return isInState(READY_STATE);
+      37             : }
+      38             : /*//}*/
+      39             : 
+      40             : /*//{ isStarted() */
+      41       32720 : bool Estimator::isStarted() const {
+      42       32720 :   return isInState(STARTED_STATE);
+      43             : }
+      44             : /*//}*/
+      45             : 
+      46             : /*//{ isRunning() */
+      47     1462559 : bool Estimator::isRunning() const {
+      48     1462559 :   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     1125902 : bool Estimator::isError() const {
+      60     1125902 :   return isInState(ERROR_STATE);
+      61             : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ getCurrentSmState() */
+      65     7441034 : SMStates_t Estimator::getCurrentSmState() const {
+      66     7441034 :   return current_sm_state_;
+      67             : }
+      68             : /*//}*/
+      69             : 
+      70             : /*//{ setCurrentSmState() */
+      71        1852 : void Estimator::setCurrentSmState(const SMStates_t& new_state) {
+      72        1852 :   std::scoped_lock lock(mutex_current_state_);
+      73        1852 :   current_sm_state_ = new_state;
+      74        1852 : }
+      75             : /*//}*/
+      76             : 
+      77             : /*//{ getSmStateString() */
+      78        3704 : std::string Estimator::getSmStateString(const SMStates_t& state) const {
+      79        3704 :   return sm::state_names[state];
+      80             : }
+      81             : /*//}*/
+      82             : 
+      83             : /*//{ getCurrentSmStateName() */
+      84        1852 : std::string Estimator::getCurrentSmStateString(void) const {
+      85        3704 :   return getSmStateString(getCurrentSmState());
+      86             : }
+      87             : /*//}*/
+      88             : 
+      89             : /*//{ isMitigatingJump() */
+      90      645822 : bool Estimator::isMitigatingJump(void) const {
+      91      645822 :   return is_mitigating_jump_;
+      92             : }
+      93             : /*//}*/
+      94             : 
+      95             : /*//{ getName() */
+      96      533883 : std::string Estimator::getName(void) const {
+      97      533883 :   return name_;
+      98             : }
+      99             : /*//}*/
+     100             : 
+     101             : /*//{ getPrintName() */
+     102        2775 : std::string Estimator::getPrintName(void) const {
+     103        5550 :   return ch_->nodelet_name + "/" + name_;
+     104             : }
+     105             : /*//}*/
+     106             : 
+     107             : /*//{ getType() */
+     108           0 : std::string Estimator::getType(void) const {
+     109           0 :   return type_;
+     110             : }
+     111             : /*//}*/
+     112             : 
+     113             : /*//{ getFrameId() */
+     114      627138 : std::string Estimator::getFrameId(void) const {
+     115      627138 :   return ns_frame_id_;
+     116             : }
+     117             : /*//}*/
+     118             : 
+     119             : /*//{ getMaxFlightZ() */
+     120       15742 : double Estimator::getMaxFlightZ(void) const {
+     121       15742 :   return max_flight_z_;
+     122             : }
+     123             : /*//}*/
+     124             : 
+     125             : /*//{ publishDiagnostics() */
+     126      886248 : void Estimator::publishDiagnostics() const {
+     127             : 
+     128      886248 :   if (!ch_->debug_topics.diag) {
+     129      886207 :     return;
+     130             :   }
+     131             : 
+     132           1 :   mrs_msgs::EstimatorDiagnostics msg;
+     133           0 :   msg.header.stamp       = ros::Time::now();
+     134           0 :   msg.header.frame_id    = getFrameId();
+     135           0 :   msg.estimator_name     = getName();
+     136           0 :   msg.estimator_type     = getType();
+     137           0 :   msg.estimator_sm_state = getCurrentSmStateString();
+     138             : 
+     139           0 :   ph_diagnostics_.publish(msg);
+     140             : }
+     141             : /*//}*/
+     142             : 
+     143             : /*//{ getAccGlobal() */
+     144           0 : tf2::Vector3 Estimator::getAccGlobal(const sensor_msgs::Imu::ConstPtr& input_msg, const double hdg) {
+     145             : 
+     146           0 :   geometry_msgs::Vector3Stamped acc_stamped;
+     147           0 :   acc_stamped.vector = input_msg->linear_acceleration;
+     148           0 :   acc_stamped.header = input_msg->header;
+     149           0 :   return getAccGlobal(acc_stamped, hdg);
+     150             : }
+     151             : 
+     152      248458 : tf2::Vector3 Estimator::getAccGlobal(const mrs_msgs::EstimatorInput::ConstPtr& input_msg, const double hdg) {
+     153             : 
+     154      496055 :   geometry_msgs::Vector3Stamped acc_stamped;
+     155      247983 :   acc_stamped.vector = input_msg->control_acceleration;
+     156      248017 :   acc_stamped.header = input_msg->header;
+     157      491874 :   return getAccGlobal(acc_stamped, hdg);
+     158             : }
+     159             : 
+     160      247817 : tf2::Vector3 Estimator::getAccGlobal(const geometry_msgs::Vector3Stamped& acc_stamped, const double hdg) {
+     161             : 
+     162             :   // untilt the desired acceleration vector
+     163      493281 :   geometry_msgs::Vector3Stamped des_acc;
+     164      247741 :   geometry_msgs::Vector3        des_acc_untilted;
+     165      247975 :   des_acc.vector.x        = acc_stamped.vector.x;
+     166      247975 :   des_acc.vector.y        = acc_stamped.vector.y;
+     167      247975 :   des_acc.vector.z        = acc_stamped.vector.z;
+     168      247975 :   des_acc.header.frame_id = ch_->frames.ns_fcu;
+     169      248028 :   des_acc.header.stamp    = acc_stamped.header.stamp;
+     170      496008 :   auto response_acc       = ch_->transformer->transformSingle(des_acc, ch_->frames.ns_fcu_untilted);
+     171      248512 :   if (response_acc) {
+     172      248512 :     des_acc_untilted.x = response_acc.value().vector.x;
+     173      248512 :     des_acc_untilted.y = response_acc.value().vector.y;
+     174      248512 :     des_acc_untilted.z = response_acc.value().vector.z;
+     175             :   } else {
+     176           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: Transform from %s to %s failed", getPrintName().c_str(), des_acc.header.frame_id.c_str(),
+     177             :                       ch_->frames.ns_fcu_untilted.c_str());
+     178             :   }
+     179             : 
+     180             :   // rotate the desired acceleration vector to global frame
+     181      248512 :   const tf2::Vector3 des_acc_global = Support::rotateVecByHdg(des_acc_untilted, hdg);
+     182             : 
+     183      491714 :   return des_acc_global;
+     184             : }
+     185             : /*//}*/
+     186             : 
+     187             : /*//{ getHeadingRate() */
+     188           0 : std::optional<double> Estimator::getHeadingRate(const nav_msgs::OdometryConstPtr& odom_msg) {
+     189             : 
+     190             :   double hdg_rate;
+     191             :   try {
+     192           0 :     hdg_rate = mrs_lib::AttitudeConverter(odom_msg->pose.pose.orientation).getHeadingRate(odom_msg->twist.twist.angular);
+     193             :   }
+     194           0 :   catch (...) {
+     195           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed getting heading rate", getPrintName().c_str());
+     196           0 :     return {};
+     197             :   }
+     198           0 :   return hdg_rate;
+     199             : }
+     200             : /*//}*/
+     201             : 
+     202             : 
+     203             : /*//}*/
+     204             : 
+     205             : }  // namespace mrs_uav_managers
+     206             : 
+
+
+
+ + + + +
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..109fe8a5e8 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimator.cpp.gcov.overview.html @@ -0,0 +1,72 @@ + + + + + + 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 + + +
+ 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..584420267ad35fffec2e7dbe5e336d5a9795a7d7 GIT binary patch literal 805 zcmV+=1KRwFP)k-cD4zfciPmoE>}EQh(Y^*=5?(;nbF;^jFm=V; z9|wtt;gb9I&;y|Jpa+OE92k!CJk5peH?$rzQ=`z-=p6vZrLzNQ%(|}2x*Eomu^hw$ z#Wp6gV3sJE%rG9O!{c4WAWiOD!%^JV4%~j_J(80ufvz0nF_Ks)Gp@?YGSe^wusj0m zynH%k&GN2Uqbnh?=4!12)@-UX)+A+&HA&|4tWQ>WgLa+kzsZ{5{uQkEwiV27AEH4i z-{F!`olDr^%>TN+mo=a7ok3B#82g-cgV~p`t`;lk+4GdKMnY7Ri9oGyDvEZP0q|0I z8k=ih6nZW!MQHLN`nsjiEDN0V4EXvv5@KI*?1eRxYwUzH)pN{Q)6_a(EfM=N*C^@8 zn&s31YcA7xh4W&+ogpV81Rb#Ea~-xZ$+kVqLT_i-%}NK`88+2euF+i$%ym13pU<`A zNGjRPmhV_ ze7Ru(M?FHIyFtY=0o@hYGW;fRjwfd{-4K|0EU^wa#rx=K?)W{mIPytP@JLs#em77z zdM^YTlgo+qnzmT?NqPhIajnckNqNKTJ*>iyudLF?C?hdLSQ_;?kqUus0{htQwVPqp zw1{hYFH}$e)|H>+AY*-0SJf~plfV2l%}3D?2n*Y$ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - agl_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:305158.8 %
Date:2024-04-09 22:03:33Functions: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&) const61
mrs_uav_managers::AglEstimator::publishAglHeight() const122028
mrs_uav_managers::AglEstimator::publishCovariance() const122028
+
+
+ + + +
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..19baa09989 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - agl_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:305158.8 %
Date:2024-04-09 22:03:33Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::AglEstimator::publishAglHeight() const122028
mrs_uav_managers::AglEstimator::publishCovariance() const122028
mrs_uav_managers::AglEstimator::isCompatibleWithHwApi(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&) const61
+
+
+ + + +
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..c2a32069a8 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp.gcov.html @@ -0,0 +1,182 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/agl_estimator.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - agl_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:305158.8 %
Date:2024-04-09 22:03:33Functions: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      122028 : void AglEstimator::publishAglHeight() const {
+       8      122028 :   ph_agl_height_.publish(mrs_lib::get_mutexed(mtx_agl_height_, agl_height_));
+       9      122028 : }
+      10             : /*//}*/
+      11             : 
+      12             : /*//{ publishCovariance() */
+      13      122028 : void AglEstimator::publishCovariance() const {
+      14             : 
+      15      122028 :   if (!ch_->debug_topics.covariance) {
+      16      122028 :     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          61 : bool AglEstimator::isCompatibleWithHwApi(const mrs_msgs::HwApiCapabilitiesConstPtr& hw_api_capabilities) const {
+      25             : 
+      26          61 :   ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      27          61 :   ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      28             : 
+      29          61 :   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          61 :   ph_->param_loader->loadParam("requires/gnss", requires_gnss);
+      35          61 :   ph_->param_loader->loadParam("requires/imu", requires_imu);
+      36          61 :   ph_->param_loader->loadParam("requires/distance_sensor", requires_distance_sensor);
+      37          61 :   ph_->param_loader->loadParam("requires/altitude", requires_altitude);
+      38          61 :   ph_->param_loader->loadParam("requires/magnetometer_heading", requires_magnetometer_heading);
+      39          61 :   ph_->param_loader->loadParam("requires/position", requires_position);
+      40          61 :   ph_->param_loader->loadParam("requires/orientation", requires_orientation);
+      41          61 :   ph_->param_loader->loadParam("requires/velocity", requires_velocity);
+      42          61 :   ph_->param_loader->loadParam("requires/angular_velocity", requires_angular_velocity);
+      43             : 
+      44          61 :   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          61 :   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          61 :   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          61 :   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          61 :   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          61 :   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          61 :   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          61 :   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          61 :   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          61 :   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          61 :   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..3219773421 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-04-09 22:03:33Functions: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..0592202675 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-04-09 22:03:33Functions: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..149889daa4 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-04-09 22:03:33Functions: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..e77a160600 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-04-09 22:03:33Functions: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..01c2e96bb1 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-04-09 22:03:33Functions: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..c181c4ec3d --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimatorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:8213560.7 %
Date:2024-04-09 22:03:33Functions: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..c7e963ff59 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - state_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:528461.9 %
Date:2024-04-09 22:03:33Functions: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&) const86
mrs_uav_managers::StateEstimator::getInnovation() const145720
mrs_uav_managers::StateEstimator::getPoseCovariance() const145720
mrs_uav_managers::StateEstimator::getTwistCovariance() const145720
mrs_uav_managers::StateEstimator::getUavState()161144
mrs_uav_managers::StateEstimator::publishUavState() const167912
mrs_uav_managers::StateEstimator::publishOdom() const167927
mrs_uav_managers::StateEstimator::publishCovariance() const167930
mrs_uav_managers::StateEstimator::publishInnovation() const167930
mrs_uav_managers::StateEstimator::rotateQuaternionByHeading(geometry_msgs::Quaternion_<std::allocator<void> > const&, double) const340371
+
+
+ + + +
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..96ed3615aa --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - state_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:528461.9 %
Date:2024-04-09 22:03:33Functions:1010100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::StateEstimator::getUavState()161144
mrs_uav_managers::StateEstimator::publishOdom() const167927
mrs_uav_managers::StateEstimator::getInnovation() const145720
mrs_uav_managers::StateEstimator::publishUavState() const167912
mrs_uav_managers::StateEstimator::getPoseCovariance() const145720
mrs_uav_managers::StateEstimator::publishCovariance() const167930
mrs_uav_managers::StateEstimator::publishInnovation() const167930
mrs_uav_managers::StateEstimator::getTwistCovariance() const145720
mrs_uav_managers::StateEstimator::isCompatibleWithHwApi(boost::shared_ptr<mrs_msgs::HwApiCapabilities_<std::allocator<void> > const> const&) const86
mrs_uav_managers::StateEstimator::rotateQuaternionByHeading(geometry_msgs::Quaternion_<std::allocator<void> > const&, double) const340371
+
+
+ + + +
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..79df774df8 --- /dev/null +++ b/mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp.gcov.html @@ -0,0 +1,263 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/estimation_manager/estimators/state_estimator.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src/estimation_manager/estimators - state_estimator.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:528461.9 %
Date:2024-04-09 22:03:33Functions: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      161144 : std::optional<mrs_msgs::UavState> StateEstimator::getUavState() {
+       9             : 
+      10      161144 :   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      322279 :   return mrs_lib::get_mutexed(mtx_uav_state_, uav_state_);
+      16             : }
+      17             : /*//}*/
+      18             : 
+      19             : /*//{ getInnovation() */
+      20      145720 : nav_msgs::Odometry StateEstimator::getInnovation() const {
+      21      145720 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_);
+      22             : }
+      23             : /*//}*/
+      24             : 
+      25             : /*//{ getPoseCovariance() */
+      26      145720 : std::vector<double> StateEstimator::getPoseCovariance() const {
+      27      145720 :   return mrs_lib::get_mutexed(mtx_covariance_, pose_covariance_.values);
+      28             : }
+      29             : /*//}*/
+      30             : 
+      31             : /*//{ getTwistCovariance() */
+      32      145720 : std::vector<double> StateEstimator::getTwistCovariance() const {
+      33      145720 :   return mrs_lib::get_mutexed(mtx_covariance_, twist_covariance_.values);
+      34             : }
+      35             : /*//}*/
+      36             : 
+      37             : /*//{ publishUavState() */
+      38      167912 : void StateEstimator::publishUavState() const {
+      39             : 
+      40      167912 :   if (!ch_->debug_topics.state) {
+      41           0 :     return;
+      42             :   }
+      43             : 
+      44      335584 :   auto uav_state = mrs_lib::get_mutexed(mtx_uav_state_, uav_state_);
+      45      167662 :   ph_uav_state_.publish(uav_state);
+      46             : }
+      47             : /*//}*/
+      48             : 
+      49             : /*//{ publishOdom() */
+      50      167927 : void StateEstimator::publishOdom() const {
+      51             : 
+      52      335857 :   auto odom = mrs_lib::get_mutexed(mtx_odom_, odom_);
+      53      167920 :   ph_odom_.publish(odom);
+      54      167930 : }
+      55             : /*//}*/
+      56             : 
+      57             : /*//{ publishCovariance() */
+      58      167930 : void StateEstimator::publishCovariance() const {
+      59             : 
+      60      167930 :   if (!ch_->debug_topics.covariance) {
+      61      167930 :     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      167930 : void StateEstimator::publishInnovation() const {
+      73             : 
+      74      167930 :   if (!ch_->debug_topics.innovation) {
+      75      167929 :     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      340371 : std::optional<geometry_msgs::Quaternion> StateEstimator::rotateQuaternionByHeading(const geometry_msgs::Quaternion& q, const double hdg) const {
+      85             : 
+      86             :   try {
+      87      340371 :     tf2::Quaternion tf2_q = mrs_lib::AttitudeConverter(q);
+      88             : 
+      89             :     // Obtain heading from quaternion
+      90      339894 :     double q_hdg = 0;
+      91      339894 :     q_hdg        = mrs_lib::AttitudeConverter(q).getHeading();
+      92             : 
+      93             :     // Build rotation matrix from difference between new heading and quaternion heading
+      94      339350 :     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      338962 :     geometry_msgs::Quaternion q_new = mrs_lib::AttitudeConverter(tf2::Transform(rot_mat) * tf2_q);
+      98      337940 :     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          86 : bool StateEstimator::isCompatibleWithHwApi(const mrs_msgs::HwApiCapabilitiesConstPtr& hw_api_capabilities) const {
+     109             : 
+     110          86 :   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          86 :   ph_->param_loader->loadParam("requires/gnss", requires_gnss);
+     116          86 :   ph_->param_loader->loadParam("requires/imu", requires_imu);
+     117          86 :   ph_->param_loader->loadParam("requires/distance_sensor", requires_distance_sensor);
+     118          86 :   ph_->param_loader->loadParam("requires/altitude", requires_altitude);
+     119          86 :   ph_->param_loader->loadParam("requires/magnetometer_heading", requires_magnetometer_heading);
+     120          86 :   ph_->param_loader->loadParam("requires/position", requires_position);
+     121          86 :   ph_->param_loader->loadParam("requires/orientation", requires_orientation);
+     122          86 :   ph_->param_loader->loadParam("requires/velocity", requires_velocity);
+     123          86 :   ph_->param_loader->loadParam("requires/angular_velocity", requires_angular_velocity);
+     124             : 
+     125          86 :   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          86 :   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          86 :   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          86 :   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          86 :   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          86 :   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          86 :   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          86 :   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          86 :   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          86 :   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          86 :   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..b4bbc1fedf --- /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:43361970.0 %
Date:2024-04-09 22:03:33Functions:404785.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
estimator.cpp +
72.9%72.9%
+ 		72.9 %62 / 8582.6 %19 / 23
<unnamed>72.9 %62 / 8582.6 %19 / 23
estimation_manager.cpp +
69.5%69.5%
+ 		69.5 %371 / 53487.5 %21 / 24
<unnamed>69.5 %371 / 53487.5 %21 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index-detail-sort-l.html b/mrs_uav_managers/src/estimation_manager/index-detail-sort-l.html new file mode 100644 index 0000000000..401fc47df7 --- /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:43361970.0 %
Date:2024-04-09 22:03:33Functions:404785.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
estimation_manager.cpp +
69.5%69.5%
+ 		69.5 %371 / 53487.5 %21 / 24
<unnamed>69.5 %371 / 53487.5 %21 / 24
estimator.cpp +
72.9%72.9%
+ 		72.9 %62 / 8582.6 %19 / 23
<unnamed>72.9 %62 / 8582.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..0b298636fc --- /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:43361970.0 %
Date:2024-04-09 22:03:33Functions:404785.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
estimation_manager.cpp +
69.5%69.5%
+ 		69.5 %371 / 53487.5 %21 / 24
<unnamed>69.5 %371 / 53487.5 %21 / 24
estimator.cpp +
72.9%72.9%
+ 		72.9 %62 / 8582.6 %19 / 23
<unnamed>72.9 %62 / 8582.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..d424785de8 --- /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:43361970.0 %
Date:2024-04-09 22:03:33Functions:404785.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
estimator.cpp +
72.9%72.9%
+ 		72.9 %62 / 8582.6 %19 / 23
estimation_manager.cpp +
69.5%69.5%
+ 		69.5 %371 / 53487.5 %21 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_managers/src/estimation_manager/index-sort-l.html b/mrs_uav_managers/src/estimation_manager/index-sort-l.html new file mode 100644 index 0000000000..e426915437 --- /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:43361970.0 %
Date:2024-04-09 22:03:33Functions:404785.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
estimation_manager.cpp +
69.5%69.5%
+ 		69.5 %371 / 53487.5 %21 / 24
estimator.cpp +
72.9%72.9%
+ 		72.9 %62 / 8582.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..bda60eb48f --- /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:43361970.0 %
Date:2024-04-09 22:03:33Functions:404785.1 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
estimation_manager.cpp +
69.5%69.5%
+ 		69.5 %371 / 53487.5 %21 / 24
estimator.cpp +
72.9%72.9%
+ 		72.9 %62 / 8582.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..821ab07a3a --- /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:20625879.8 %
Date:2024-04-09 22:03:33Functions:6785.7 %
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> >&)0
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_managers::gain_manager::GainManager::onInit()82
mrs_uav_managers::gain_manager::GainManager::setGains(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)82
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&)1808
mrs_uav_managers::gain_manager::GainManager::timerDiagnostics(ros::TimerEvent const&)1921
mrs_uav_managers::gain_manager::GainManager::timerGainManagement(ros::TimerEvent const&)19603
+
+
+ + + +
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..f2b160e246 --- /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:20625879.8 %
Date:2024-04-09 22:03:33Functions:6785.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()82
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&)1808
mrs_uav_managers::gain_manager::GainManager::callbackSetGains(mrs_msgs::StringRequest_<std::allocator<void> >&, mrs_msgs::StringResponse_<std::allocator<void> >&)0
mrs_uav_managers::gain_manager::GainManager::timerDiagnostics(ros::TimerEvent const&)1921
mrs_uav_managers::gain_manager::GainManager::timerGainManagement(ros::TimerEvent const&)19603
mrs_uav_managers::gain_manager::GainManager::onInit()82
mrs_uav_managers::gain_manager::GainManager::setGains(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)82
+
+
+ + + +
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..33e1160c59 --- /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:20625879.8 %
Date:2024-04-09 22:03:33Functions:6785.7 %
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          82 : void GainManager::onInit() {
+     123             : 
+     124         164 :   ros::NodeHandle nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     125             : 
+     126          82 :   ros::Time::waitForValid();
+     127             : 
+     128          82 :   ROS_INFO("[GainManager]: initializing");
+     129             : 
+     130             :   // | ------------------------- params ------------------------- |
+     131             : 
+     132         164 :   mrs_lib::ParamLoader param_loader(nh_, "GainManager");
+     133             : 
+     134         164 :   std::string custom_config_path;
+     135         164 :   std::string platform_config_path;
+     136             : 
+     137          82 :   param_loader.loadParam("custom_config", custom_config_path);
+     138          82 :   param_loader.loadParam("platform_config", platform_config_path);
+     139             : 
+     140          82 :   if (custom_config_path != "") {
+     141          82 :     param_loader.addYamlFile(custom_config_path);
+     142             :   }
+     143             : 
+     144          82 :   if (platform_config_path != "") {
+     145          82 :     param_loader.addYamlFile(platform_config_path);
+     146             :   }
+     147             : 
+     148          82 :   param_loader.addYamlFileFromParam("private_config");
+     149          82 :   param_loader.addYamlFileFromParam("public_config");
+     150          82 :   param_loader.addYamlFileFromParam("public_gains");
+     151             : 
+     152         164 :   const std::string yaml_prefix = "mrs_uav_managers/gain_manager/";
+     153             : 
+     154             :   // params passed from the launch file are not prefixed
+     155          82 :   param_loader.loadParam("enable_profiler", _profiler_enabled_);
+     156             : 
+     157          82 :   param_loader.loadParam(yaml_prefix + "gains", _gain_names_);
+     158             : 
+     159          82 :   param_loader.loadParam(yaml_prefix + "estimator_types", _current_state_estimators_);
+     160             : 
+     161          82 :   param_loader.loadParam(yaml_prefix + "rate", _gain_management_rate_);
+     162          82 :   param_loader.loadParam(yaml_prefix + "diagnostics_rate", _diagnostics_rate_);
+     163             : 
+     164             :   // | ------------------- scope timer logger ------------------- |
+     165             : 
+     166          82 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_);
+     167         246 :   const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+     168          82 :   scope_timer_logger_                        = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+     169             : 
+     170          82 :   std::vector<std::string>::iterator it;
+     171             : 
+     172             :   // loading gain_names
+     173         246 :   for (it = _gain_names_.begin(); it != _gain_names_.end(); ++it) {
+     174             : 
+     175         164 :     ROS_INFO_STREAM("[GainManager]: loading gains '" << *it << "'");
+     176             : 
+     177         164 :     Gains_t new_gains;
+     178             : 
+     179         164 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kp", new_gains.kpxy);
+     180         164 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kv", new_gains.kvxy);
+     181         164 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/ka", new_gains.kaxy);
+     182         164 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kib", new_gains.kibxy);
+     183         164 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kiw", new_gains.kiwxy);
+     184         164 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kib_lim", new_gains.kibxy_lim);
+     185         164 :     param_loader.loadParam(yaml_prefix + *it + "/horizontal/kiw_lim", new_gains.kiwxy_lim);
+     186             : 
+     187         164 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/kp", new_gains.kpz);
+     188         164 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/kv", new_gains.kvz);
+     189         164 :     param_loader.loadParam(yaml_prefix + *it + "/vertical/ka", new_gains.kaz);
+     190             : 
+     191         164 :     param_loader.loadParam(yaml_prefix + *it + "/attitude/kq_roll_pitch", new_gains.kqrp);
+     192         164 :     param_loader.loadParam(yaml_prefix + *it + "/attitude/kq_yaw", new_gains.kqy);
+     193             : 
+     194         164 :     param_loader.loadParam(yaml_prefix + *it + "/mass_estimator/km", new_gains.km);
+     195         164 :     param_loader.loadParam(yaml_prefix + *it + "/mass_estimator/km_lim", new_gains.km_lim);
+     196             : 
+     197         164 :     _gains_.insert(std::pair<std::string, Gains_t>(*it, new_gains));
+     198             :   }
+     199             : 
+     200             :   // loading the allowed gains lists
+     201         656 :   for (it = _current_state_estimators_.begin(); it != _current_state_estimators_.end(); ++it) {
+     202             : 
+     203         574 :     std::vector<std::string> temp_vector;
+     204         574 :     param_loader.loadParam(yaml_prefix + "allowed_gains/" + *it, temp_vector);
+     205             : 
+     206         574 :     std::vector<std::string>::iterator it2;
+     207        1722 :     for (it2 = temp_vector.begin(); it2 != temp_vector.end(); ++it2) {
+     208        1148 :       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         574 :     _map_type_allowed_gains_.insert(std::pair<std::string, std::vector<std::string>>(*it, temp_vector));
+     215             :   }
+     216             : 
+     217             :   // loading the default gains
+     218         656 :   for (it = _current_state_estimators_.begin(); it != _current_state_estimators_.end(); ++it) {
+     219             : 
+     220         574 :     std::string temp_str;
+     221         574 :     param_loader.loadParam(yaml_prefix + "default_gains/" + *it, temp_str);
+     222             : 
+     223         574 :     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         574 :     _map_type_default_gains_.insert(std::pair<std::string, std::string>(*it, temp_str));
+     229             :   }
+     230             : 
+     231          82 :   ROS_INFO("[GainManager]: done loading dynamical params");
+     232             : 
+     233          82 :   current_gains_       = "";
+     234          82 :   last_estimator_name_ = "";
+     235             : 
+     236             :   // | ------------------------ services ------------------------ |
+     237             : 
+     238          82 :   service_server_set_gains_ = nh_.advertiseService("set_gains_in", &GainManager::callbackSetGains, this);
+     239             : 
+     240          82 :   service_client_set_gains_ = nh_.serviceClient<dynamic_reconfigure::Reconfigure>("set_gains_out");
+     241             : 
+     242             :   // | ----------------------- subscribers ---------------------- |
+     243             : 
+     244         164 :   mrs_lib::SubscribeHandlerOptions shopts;
+     245          82 :   shopts.nh                 = nh_;
+     246          82 :   shopts.node_name          = "GainManager";
+     247          82 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     248          82 :   shopts.threadsafe         = true;
+     249          82 :   shopts.autostart          = true;
+     250          82 :   shopts.queue_size         = 10;
+     251          82 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     252             : 
+     253          82 :   sh_estimation_diag_      = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "estimation_diagnostics_in");
+     254          82 :   sh_control_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "control_manager_diagnostics_in");
+     255             : 
+     256             :   // | ----------------------- publishers ----------------------- |
+     257             : 
+     258          82 :   ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::GainManagerDiagnostics>(nh_, "diagnostics_out", 10);
+     259             : 
+     260             :   // | ------------------------- timers ------------------------- |
+     261             : 
+     262          82 :   timer_gain_management_ = nh_.createTimer(ros::Rate(_gain_management_rate_), &GainManager::timerGainManagement, this);
+     263          82 :   timer_diagnostics_     = nh_.createTimer(ros::Rate(_diagnostics_rate_), &GainManager::timerDiagnostics, this);
+     264             : 
+     265             :   // | ------------------------ profiler ------------------------ |
+     266             : 
+     267          82 :   profiler_ = mrs_lib::Profiler(nh_, "GainManager", _profiler_enabled_);
+     268             : 
+     269             :   // | ----------------------- finish init ---------------------- |
+     270             : 
+     271          82 :   if (!param_loader.loadedSuccessfully()) {
+     272           0 :     ROS_ERROR("[GainManager]: could not load all parameters!");
+     273           0 :     ros::shutdown();
+     274             :   }
+     275             : 
+     276          82 :   is_initialized_ = true;
+     277             : 
+     278          82 :   ROS_INFO("[GainManager]: initialized");
+     279             : 
+     280          82 :   ROS_DEBUG("[GainManager]: debug output is enabled");
+     281          82 : }
+     282             : 
+     283             : //}
+     284             : 
+     285             : // --------------------------------------------------------------
+     286             : // |                           methods                          |
+     287             : // --------------------------------------------------------------
+     288             : 
+     289             : /* setGains() //{ */
+     290             : 
+     291          82 : bool GainManager::setGains(std::string gains_name) {
+     292             : 
+     293          82 :   std::map<std::string, Gains_t>::iterator it;
+     294          82 :   it = _gains_.find(gains_name);
+     295             : 
+     296          82 :   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         164 :   dynamic_reconfigure::Config          conf;
+     302         164 :   dynamic_reconfigure::DoubleParameter param;
+     303             : 
+     304          82 :   param.name  = "kpxy";
+     305          82 :   param.value = it->second.kpxy;
+     306          82 :   conf.doubles.push_back(param);
+     307             : 
+     308          82 :   param.name  = "kvxy";
+     309          82 :   param.value = it->second.kvxy;
+     310          82 :   conf.doubles.push_back(param);
+     311             : 
+     312          82 :   param.name  = "kaxy";
+     313          82 :   param.value = it->second.kaxy;
+     314          82 :   conf.doubles.push_back(param);
+     315             : 
+     316          82 :   param.name  = "kq_roll_pitch";
+     317          82 :   param.value = it->second.kqrp;
+     318          82 :   conf.doubles.push_back(param);
+     319             : 
+     320          82 :   param.name  = "kibxy";
+     321          82 :   param.value = it->second.kibxy;
+     322          82 :   conf.doubles.push_back(param);
+     323             : 
+     324          82 :   param.name  = "kiwxy";
+     325          82 :   param.value = it->second.kiwxy;
+     326          82 :   conf.doubles.push_back(param);
+     327             : 
+     328          82 :   param.name  = "kibxy_lim";
+     329          82 :   param.value = it->second.kibxy_lim;
+     330          82 :   conf.doubles.push_back(param);
+     331             : 
+     332          82 :   param.name  = "kiwxy_lim";
+     333          82 :   param.value = it->second.kiwxy_lim;
+     334          82 :   conf.doubles.push_back(param);
+     335             : 
+     336          82 :   param.name  = "kpz";
+     337          82 :   param.value = it->second.kpz;
+     338          82 :   conf.doubles.push_back(param);
+     339             : 
+     340          82 :   param.name  = "kvz";
+     341          82 :   param.value = it->second.kvz;
+     342          82 :   conf.doubles.push_back(param);
+     343             : 
+     344          82 :   param.name  = "kaz";
+     345          82 :   param.value = it->second.kaz;
+     346          82 :   conf.doubles.push_back(param);
+     347             : 
+     348          82 :   param.name  = "kq_yaw";
+     349          82 :   param.value = it->second.kqy;
+     350          82 :   conf.doubles.push_back(param);
+     351             : 
+     352          82 :   param.name  = "km";
+     353          82 :   param.value = it->second.km;
+     354          82 :   conf.doubles.push_back(param);
+     355             : 
+     356          82 :   param.name  = "km_lim";
+     357          82 :   param.value = it->second.km_lim;
+     358          82 :   conf.doubles.push_back(param);
+     359             : 
+     360         164 :   dynamic_reconfigure::ReconfigureRequest  srv_req;
+     361         164 :   dynamic_reconfigure::ReconfigureResponse srv_resp;
+     362             : 
+     363          82 :   srv_req.config = conf;
+     364             : 
+     365         164 :   dynamic_reconfigure::Reconfigure reconf;
+     366          82 :   reconf.request = srv_req;
+     367             : 
+     368          82 :   ROS_INFO_THROTTLE(1.0, "[GainManager]: setting up gains for '%s'", gains_name.c_str());
+     369             : 
+     370          82 :   bool res = service_client_set_gains_.call(reconf);
+     371             : 
+     372          82 :   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          82 :     mrs_lib::set_mutexed(mutex_current_gains_, gains_name, current_gains_);
+     380             : 
+     381          82 :     return true;
+     382             :   }
+     383             : }
+     384             : 
+     385             : //}
+     386             : 
+     387             : // --------------------------------------------------------------
+     388             : // |                          callbacks                         |
+     389             : // --------------------------------------------------------------
+     390             : 
+     391             : // | -------------------- service callbacks ------------------- |
+     392             : 
+     393             : /* //{ callbackSetGains() */
+     394             : 
+     395           0 : bool GainManager::callbackSetGains(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+     396             : 
+     397           0 :   if (!is_initialized_) {
+     398           0 :     return false;
+     399             :   }
+     400             : 
+     401           0 :   std::stringstream ss;
+     402             : 
+     403           0 :   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           0 :   auto estimation_diagnostics = sh_estimation_diag_.getMsg();
+     415             : 
+     416           0 :   if (!stringInVector(req.value, _gain_names_)) {
+     417             : 
+     418           0 :     ss << "the gains '" << req.value.c_str() << "' do not exist (in the GainManager's config)";
+     419             : 
+     420           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[GainManager]: " << ss.str());
+     421             : 
+     422           0 :     res.message = ss.str();
+     423           0 :     res.success = false;
+     424           0 :     return true;
+     425             :   }
+     426             : 
+     427           0 :   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           0 :   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           0 :     ss << "the gains '" << req.value.c_str() << "' are set";
+     452             : 
+     453           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[GainManager]: " << ss.str());
+     454             : 
+     455           0 :     res.message = ss.str();
+     456           0 :     res.success = true;
+     457           0 :     return true;
+     458             :   }
+     459             : }
+     460             : 
+     461             : //}
+     462             : 
+     463             : // --------------------------------------------------------------
+     464             : // |                           timers                           |
+     465             : // --------------------------------------------------------------
+     466             : 
+     467             : /* timerGainManagement() //{ */
+     468             : 
+     469       19603 : void GainManager::timerGainManagement(const ros::TimerEvent& event) {
+     470             : 
+     471       19603 :   if (!is_initialized_) {
+     472        4237 :     return;
+     473             :   }
+     474             : 
+     475       39206 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("gainManagementTimer", _gain_management_rate_, 0.01, event);
+     476       39206 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("GainManager::gainManagementTimer", scope_timer_logger_, scope_timer_enabled_);
+     477             : 
+     478       19603 :   if (!sh_estimation_diag_.hasMsg()) {
+     479        4237 :     return;
+     480             :   }
+     481             : 
+     482       15366 :   if (!sh_control_manager_diag_.hasMsg()) {
+     483           0 :     return;
+     484             :   }
+     485             : 
+     486       30732 :   auto estimation_diagnostics = sh_estimation_diag_.getMsg();
+     487             : 
+     488       30732 :   auto control_manager_diagnostics = sh_estimation_diag_.getMsg();
+     489             : 
+     490       30732 :   auto current_gains       = mrs_lib::get_mutexed(mutex_current_gains_, current_gains_);
+     491       15366 :   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       15366 :   if (estimation_diagnostics->current_state_estimator != last_estimator_name) {
+     495             : 
+     496          86 :     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          86 :     std::map<std::string, std::string>::iterator it;
+     500          86 :     it = _map_type_default_gains_.find(estimation_diagnostics->current_state_estimator);
+     501             : 
+     502          86 :     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          86 :       if (stringInVector(current_gains, _map_type_allowed_gains_.at(estimation_diagnostics->current_state_estimator))) {
+     511             : 
+     512           4 :         last_estimator_name = estimation_diagnostics->current_state_estimator;
+     513             : 
+     514             :         // else, try to set the default gains
+     515             :       } else {
+     516             : 
+     517          82 :         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          82 :         if (setGains(it->second)) {
+     521             : 
+     522          82 :           last_estimator_name = estimation_diagnostics->current_state_estimator;
+     523             : 
+     524          82 :           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       15366 :   mrs_lib::set_mutexed(mutex_last_estimator_name_, last_estimator_name, last_estimator_name_);
+     535             : }
+     536             : 
+     537             : //}
+     538             : 
+     539             : /* timerDiagnostics() //{ */
+     540             : 
+     541        1921 : void GainManager::timerDiagnostics(const ros::TimerEvent& event) {
+     542             : 
+     543        1921 :   if (!is_initialized_) {
+     544         412 :     return;
+     545             :   }
+     546             : 
+     547        3842 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("timerDiagnostics", _diagnostics_rate_, 0.01, event);
+     548        3842 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("GainManager::timerDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+     549             : 
+     550        1921 :   if (!sh_estimation_diag_.hasMsg()) {
+     551         408 :     ROS_WARN_THROTTLE(10.0, "[GainManager]: can not do gain management, missing estimator diagnostics!");
+     552         408 :     return;
+     553             :   }
+     554             : 
+     555        1513 :   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        1513 :   auto current_gains = mrs_lib::get_mutexed(mutex_current_gains_, current_gains_);
+     561             : 
+     562        1513 :   if (current_gains == "") { // this could happend just before timerGainManagement() finishes
+     563           4 :     return;
+     564             :   }
+     565             : 
+     566        1509 :   auto estimation_diagnostics = sh_estimation_diag_.getMsg();
+     567             : 
+     568        1509 :   mrs_msgs::GainManagerDiagnostics diagnostics;
+     569             : 
+     570        1509 :   diagnostics.stamp        = ros::Time::now();
+     571        1509 :   diagnostics.current_name = current_gains;
+     572        1509 :   diagnostics.loaded       = _gain_names_;
+     573             : 
+     574             :   // get the available gains
+     575             :   {
+     576        1509 :     std::map<std::string, std::vector<std::string>>::iterator it;
+     577        1509 :     it = _map_type_allowed_gains_.find(estimation_diagnostics->current_state_estimator);
+     578             : 
+     579        1509 :     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        1509 :       diagnostics.available = it->second;
+     584             :     }
+     585             :   }
+     586             : 
+     587             :   // get the current gain values
+     588             :   {
+     589        1509 :     std::map<std::string, Gains_t>::iterator it;
+     590        1509 :     it = _gains_.find(current_gains);
+     591             : 
+     592        1509 :     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        1509 :     diagnostics.current_values.kpxy = it->second.kpxy;
+     598        1509 :     diagnostics.current_values.kvxy = it->second.kvxy;
+     599        1509 :     diagnostics.current_values.kaxy = it->second.kaxy;
+     600             : 
+     601        1509 :     diagnostics.current_values.kqrp = it->second.kqrp;
+     602             : 
+     603        1509 :     diagnostics.current_values.kibxy     = it->second.kibxy;
+     604        1509 :     diagnostics.current_values.kibxy_lim = it->second.kibxy_lim;
+     605             : 
+     606        1509 :     diagnostics.current_values.kiwxy     = it->second.kiwxy;
+     607        1509 :     diagnostics.current_values.kiwxy_lim = it->second.kiwxy_lim;
+     608             : 
+     609        1509 :     diagnostics.current_values.kpz = it->second.kpz;
+     610        1509 :     diagnostics.current_values.kvz = it->second.kvz;
+     611        1509 :     diagnostics.current_values.kaz = it->second.kaz;
+     612             : 
+     613        1509 :     diagnostics.current_values.kqy = it->second.kqy;
+     614             : 
+     615        1509 :     diagnostics.current_values.km     = it->second.km;
+     616        1509 :     diagnostics.current_values.km_lim = it->second.km_lim;
+     617             :   }
+     618             : 
+     619        1509 :   ph_diagnostics_.publish(diagnostics);
+     620             : }
+     621             : 
+     622             : //}
+     623             : 
+     624             : // --------------------------------------------------------------
+     625             : // |                          routines                          |
+     626             : // --------------------------------------------------------------
+     627             : 
+     628             : /* stringInVector() //{ */
+     629             : 
+     630        1808 : bool GainManager::stringInVector(const std::string& value, const std::vector<std::string>& vector) {
+     631             : 
+     632        1808 :   if (std::find(vector.begin(), vector.end(), value) == vector.end()) {
+     633          82 :     return false;
+     634             :   } else {
+     635        1726 :     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          82 : 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..d110525c20af4a9b9096484c1d30854f1a2f9740 GIT binary patch literal 2126 zcmV-U2(kBxP)t0{{R3FUJ9e0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vppld4^h(U3noG=)3MAvK}NizZRd91)^6kcP1(FhZk-Lp-I z04wHU@i_T?gyP8*M}y)$!>;SV3%XuT!&uuaBIKPcZJ@j`H|u7L<-oqG09W(Gq3F%i z0xk@}J!f*_Z;nARimN~nNt;*Tx`Ifs&M?dmAyzqyoJ@R5UQju0ZjoeE=p4lbM&<<) zA2iaI)2mGl-4x$%7m^y_+xI>@(WDgdHz*L{-2kr$;y}#{_&0CD#C}ubwRAH`eA3N| z7Mx^j03#Qp!Yt0Ie_D?@i~}EdK9wq>5}3tQFtJdXIGgCt2{!A-7+E+lW4Z4 zj7;f~k!WV$FuX{G<^^WZ6T%=L!uaS#M7@T!hUW;g9)U2sHi1u9yf4fs8a$tw8;V7h;Bzer4yo5)xWnGgv%>@=x zGljW(UvbYx0P2oCx40d(g1_Y!OUrSGgvpvxfF=M=Qp{Q1rzrw&wkof74d8k(;)>)w zRUUzYayLW35eT*58U~zDI65PCdbry-YN#ij()AgsfOd5M#GXr|9VHV3Pq!GNXt`hS30iYHKdT0+&Mvo!0xdbR=YcF>j=w3;7tvtmxk7~AS?=kL=2a4wuKK3jj zgfnALU{j?Zj!JFShsj4M-1V=6sVChG>-z-iWL|MmF&amCV3I+4ofPZ2+vD9{pdIhW z`TTkw{-gJ+<4AyyN;`o^4G@6xJLhCF94+dc;%djO@OYKuny=U7;yJGFoC}Lsm~~)* zOfP#uVBLF|UiOgo9(9HHWT8g~b7nT}PYdrRyJP^ZGt_|GuH`pV6ZK%5Nd2>Gi{jo( zQT{-za7VQhds2y)Xfo4{6g@D$VXB7&YDbaWr^vf*0v*N1o|GqwMAQIewsdh;uE~H? zBvz>bxn0Yj+J{|N+PUX=l-<#+s8K_O(}vMA!bk$nxz|%eA({VoA zgx&oAt2MkY3;IYRa z(ciT~b374YsvB&(FSn5H8lF!oZO?ST=^meAe(Hx>y|Ye=td1I?m@F!uU~Lhf&5Mpj z1+$quP)mDB(b0(G0+rab_cUeS8{Tb!+yjUUuBLpOU4>$Q;L=@%v}8D~vL7l!ULuz= zdlhj+VeJV2%^x$5Nb#tR#82)4JfeVA+Cx(N2sKOb;m$dq&JxKM>LA68MgP|wsE$%R z4uUrn*rxbw?F(P!6u^eZ@k(0kZrmySK6uW0w^KaL$#)=rzCQeZ`_VaE|D2-k6sQS{ zCSk%No#9hVT{XJ!6WZXHn$XOeuG+dffEE@ZXO&2i+0I?A#4wo_CMe#i+9K`U43zUz z)k?az25AYAVhg{`;D5o%e6+5pt$lZ(7QExG)E(b|RPK#=brOdZ2^MVM9ZrFkKa>R! zrns1{t&6R*Tsf<=#zsxgjwZRK`LwU>LM}-aUX{kz;{~bI>5;47;PST{!(}j2e`acV z?s?reR{7m&8Q>Ol$zo6O+|5qKam7Hd8iW;@2$KyjJH)hkq{7rZyszsd=2jHWwhYHK zqZE_#4?j9pXCM`~VLndd$PUZCmz^{pyo(5iMP}#HICdj_5MDSsZOyJ$WVb8V3R2a8 zSRSr`UVM`Hmxq5O4^Lcl-(8SZ;AXb|ISG5Q_h24R_ukx|`W~5mC1oD7wKnJ2diTkw z0>|OIzwi37p&-AW$s9Z*HBRxQ5slO6+5^!`r#m3xfoRuU9_{I;PbzB~=gtb$qQZXy zE(3jRQBhed*oA^iEqc}p%&h00=tongK84{8n#+E=IIeKY8uIW5_YT+&?ymMC$)1jtwT)f9I;o}z_7(4BsE*MZy@0Offcq^OD8 zv53O)GR&{}xh=GhAGSE_JILAciQy{_a>QKW)g8%>k07*qoM6N<$ Ef^5zMYybcN literal 0 HcmV?d00001 diff --git a/mrs_uav_managers/src/index-detail-sort-f.html b/mrs_uav_managers/src/index-detail-sort-f.html new file mode 100644 index 0000000000..997e211c1a --- /dev/null +++ b/mrs_uav_managers/src/index-detail-sort-f.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:1145172966.2 %
Date:2024-04-09 22:03:33Functions:587478.4 %
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
constraint_manager.cpp +
65.5%65.5%
+ 		65.5 %152 / 23275.0 %6 / 8
<unnamed>65.5 %152 / 23275.0 %6 / 8
gain_manager.cpp +
79.8%79.8%
+ 		79.8 %206 / 25885.7 %6 / 7
<unnamed>79.8 %206 / 25885.7 %6 / 7
uav_manager.cpp +
63.2%63.2%
+ 		63.2 %746 / 118089.7 %35 / 39
<unnamed>63.2 %746 / 118089.7 %35 / 39
+
+
+ + + + +
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..78c7d40dbe --- /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:1145172966.2 %
Date:2024-04-09 22:03:33Functions:587478.4 %
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 +
63.2%63.2%
+ 		63.2 %746 / 118089.7 %35 / 39
<unnamed>63.2 %746 / 118089.7 %35 / 39
constraint_manager.cpp +
65.5%65.5%
+ 		65.5 %152 / 23275.0 %6 / 8
<unnamed>65.5 %152 / 23275.0 %6 / 8
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 +
79.8%79.8%
+ 		79.8 %206 / 25885.7 %6 / 7
<unnamed>79.8 %206 / 25885.7 %6 / 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..bc5759a56f --- /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:1145172966.2 %
Date:2024-04-09 22:03:33Functions:587478.4 %
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 +
65.5%65.5%
+ 		65.5 %152 / 23275.0 %6 / 8
<unnamed>65.5 %152 / 23275.0 %6 / 8
gain_manager.cpp +
79.8%79.8%
+ 		79.8 %206 / 25885.7 %6 / 7
<unnamed>79.8 %206 / 25885.7 %6 / 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 +
63.2%63.2%
+ 		63.2 %746 / 118089.7 %35 / 39
<unnamed>63.2 %746 / 118089.7 %35 / 39
+
+
+ + + + +
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..e3a1a9252f --- /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:1145172966.2 %
Date:2024-04-09 22:03:33Functions:587478.4 %
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
constraint_manager.cpp +
65.5%65.5%
+ 		65.5 %152 / 23275.0 %6 / 8
gain_manager.cpp +
79.8%79.8%
+ 		79.8 %206 / 25885.7 %6 / 7
uav_manager.cpp +
63.2%63.2%
+ 		63.2 %746 / 118089.7 %35 / 39
+
+
+ + + + +
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..f91543ac96 --- /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:1145172966.2 %
Date:2024-04-09 22:03:33Functions:587478.4 %
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 +
63.2%63.2%
+ 		63.2 %746 / 118089.7 %35 / 39
constraint_manager.cpp +
65.5%65.5%
+ 		65.5 %152 / 23275.0 %6 / 8
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
gain_manager.cpp +
79.8%79.8%
+ 		79.8 %206 / 25885.7 %6 / 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..1bfe1a0aab --- /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:1145172966.2 %
Date:2024-04-09 22:03:33Functions:587478.4 %
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 +
65.5%65.5%
+ 		65.5 %152 / 23275.0 %6 / 8
gain_manager.cpp +
79.8%79.8%
+ 		79.8 %206 / 25885.7 %6 / 7
null_tracker.cpp +
69.5%69.5%
+ 		69.5 %41 / 5955.0 %11 / 20
uav_manager.cpp +
63.2%63.2%
+ 		63.2 %746 / 118089.7 %35 / 39
+
+
+ + + + +
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..4ce1d379f8 --- /dev/null +++ b/mrs_uav_managers/src/null_tracker.cpp.func-sort-c.html @@ -0,0 +1,160 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/null_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - null_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:415969.5 %
Date:2024-04-09 22:03:33Functions: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&)4
(anonymous namespace)::ProxyExec0::ProxyExec0()82
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>)82
mrs_uav_managers::NullTracker::~NullTracker()82
mrs_uav_managers::NullTracker::~NullTracker().282
mrs_uav_managers::NullTracker::deactivate()184
mrs_uav_managers::NullTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)188
mrs_uav_managers::NullTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)255
mrs_uav_managers::NullTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)281
mrs_uav_managers::NullTracker::getStatus()5967
mrs_uav_managers::NullTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)111592
+
+
+ + + +
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..2e0b08da7c --- /dev/null +++ b/mrs_uav_managers/src/null_tracker.cpp.func.html @@ -0,0 +1,160 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/null_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - null_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:415969.5 %
Date:2024-04-09 22:03:33Functions:112055.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_managers::NullTracker::deactivate()184
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>)82
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&)255
mrs_uav_managers::NullTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)281
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&)4
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&)111592
mrs_uav_managers::NullTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)188
mrs_uav_managers::NullTracker::getStatus()5967
mrs_uav_managers::NullTracker::~NullTracker()82
mrs_uav_managers::NullTracker::~NullTracker().282
+
+
+ + + +
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..7509b490b5 --- /dev/null +++ b/mrs_uav_managers/src/null_tracker.cpp.gcov.html @@ -0,0 +1,332 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/null_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_managers/src - null_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:415969.5 %
Date:2024-04-09 22:03:33Functions: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         164 :   ~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          82 : 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         164 :   ros::NodeHandle nh_(parent_nh, "null_tracker");
+      59             : 
+      60          82 :   ros::Time::waitForValid();
+      61             : 
+      62          82 :   is_initialized = true;
+      63             : 
+      64          82 :   this->common_handlers = common_handlers;
+      65             : 
+      66          82 :   ROS_INFO("[NullTracker]: initialized");
+      67             : 
+      68         164 :   return true;
+      69             : }
+      70             : 
+      71             : //}
+      72             : 
+      73             : /* //{ activate() */
+      74             : 
+      75         188 : std::tuple<bool, std::string> NullTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+      76             : 
+      77         188 :   std::stringstream ss;
+      78         188 :   ss << "activated";
+      79             : 
+      80         188 :   ROS_INFO_STREAM("[NullTracker]: " << ss.str());
+      81         188 :   is_active = true;
+      82             : 
+      83         376 :   return std::tuple(true, ss.str());
+      84             : }
+      85             : 
+      86             : //}
+      87             : 
+      88             : /* //{ deactivate() */
+      89             : 
+      90         184 : void NullTracker::deactivate(void) {
+      91             : 
+      92         184 :   ROS_INFO("[NullTracker]: deactivated");
+      93         184 :   is_active = false;
+      94         184 : }
+      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      111592 : 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      111592 :   return {};
+     120             : }
+     121             : 
+     122             : //}
+     123             : 
+     124             : /* //{ getStatus() */
+     125             : 
+     126        5967 : const mrs_msgs::TrackerStatus NullTracker::getStatus() {
+     127             : 
+     128        5967 :   mrs_msgs::TrackerStatus tracker_status;
+     129             : 
+     130        5967 :   tracker_status.active            = is_active;
+     131        5967 :   tracker_status.callbacks_enabled = callbacks_enabled;
+     132             : 
+     133        5967 :   return tracker_status;
+     134             : }
+     135             : 
+     136             : //}
+     137             : 
+     138             : /* //{ enableCallbacks() */
+     139             : 
+     140         281 : const std_srvs::SetBoolResponse::ConstPtr NullTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     141             : 
+     142         562 :   std_srvs::SetBoolResponse res;
+     143             : 
+     144         281 :   std::stringstream ss;
+     145             : 
+     146         281 :   if (cmd->data != callbacks_enabled) {
+     147             : 
+     148           9 :     callbacks_enabled = cmd->data;
+     149             : 
+     150           9 :     ss << "callbacks " << (callbacks_enabled ? "enabled" : "disabled");
+     151             : 
+     152           9 :     ROS_DEBUG_STREAM("[NullTracker]: " << ss.str());
+     153             : 
+     154             :   } else {
+     155             : 
+     156         272 :     ss << "callbacks were already " << (callbacks_enabled ? "enabled" : "disabled");
+     157             :   }
+     158             : 
+     159         281 :   res.message = ss.str();
+     160         281 :   res.success = true;
+     161             : 
+     162         843 :   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           4 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr NullTracker::setTrajectoryReference([
+     187             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     188           4 :   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         255 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr NullTracker::setConstraints([
+     238             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     239             : 
+     240         255 :   return mrs_msgs::DynamicsConstraintsSrvResponse::Ptr();
+     241             : }
+     242             : 
+     243             : //}
+     244             : 
+     245             : }  // namespace mrs_uav_managers
+     246             : 
+     247             : #include <pluginlib/class_list_macros.h>
+     248          82 : 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:35442982.5 %
Date:2024-04-09 22:03:33Functions: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 +
82.5%82.5%
+ 		82.5 %354 / 42992.9 %13 / 14
<unnamed>82.5 %354 / 42992.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..ecfc79f2bc --- /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:35442982.5 %
Date:2024-04-09 22:03:33Functions: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 +
82.5%82.5%
+ 		82.5 %354 / 42992.9 %13 / 14
<unnamed>82.5 %354 / 42992.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..d4e19a914b --- /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:35442982.5 %
Date:2024-04-09 22:03:33Functions: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 +
82.5%82.5%
+ 		82.5 %354 / 42992.9 %13 / 14
<unnamed>82.5 %354 / 42992.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..967a894f1a --- /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:35442982.5 %
Date:2024-04-09 22:03:33Functions: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 +
82.5%82.5%
+ 		82.5 %354 / 42992.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..979eb24907 --- /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:35442982.5 %
Date:2024-04-09 22:03:33Functions: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 +
82.5%82.5%
+ 		82.5 %354 / 42992.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..4be63cf999 --- /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:35442982.5 %
Date:2024-04-09 22:03:33Functions: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 +
82.5%82.5%
+ 		82.5 %354 / 42992.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..de0486d687 --- /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:35442982.5 %
Date:2024-04-09 22:03:33Functions: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()82
mrs_uav_managers::transform_manager::TransformManager::publishLocalTf()82
mrs_uav_managers::transform_manager::TransformManager::onInit()82
mrs_uav_managers::transform_manager::TransformManager::TransformManager()82
mrs_uav_managers::transform_manager::TransformManager::getPrintName[abi:cxx11]() const1329
mrs_uav_managers::transform_manager::TransformManager::callbackRtkGps(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)3792
mrs_uav_managers::transform_manager::TransformManager::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)87120
mrs_uav_managers::transform_manager::TransformManager::callbackHeightAgl(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)128083
mrs_uav_managers::transform_manager::TransformManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)145673
mrs_uav_managers::transform_manager::TransformManager::callbackAltitudeAmsl(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)146902
mrs_uav_managers::transform_manager::TransformManager::publishFcuUntiltedTf(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)185372
mrs_uav_managers::transform_manager::TransformManager::callbackHwApiOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)185372
+
+
+ + + +
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..14c70f9be7 --- /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:35442982.5 %
Date:2024-04-09 22:03:33Functions:131492.9 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_managers::transform_manager::TransformManager::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)87120
mrs_uav_managers::transform_manager::TransformManager::callbackRtkGps(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)3792
mrs_uav_managers::transform_manager::TransformManager::publishLocalTf()82
mrs_uav_managers::transform_manager::TransformManager::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)145673
mrs_uav_managers::transform_manager::TransformManager::callbackHeightAgl(boost::shared_ptr<mrs_msgs::Float64Stamped_<std::allocator<void> > const>)128083
mrs_uav_managers::transform_manager::TransformManager::callbackAltitudeAmsl(boost::shared_ptr<mrs_msgs::HwApiAltitude_<std::allocator<void> > const>)146902
mrs_uav_managers::transform_manager::TransformManager::publishFcuUntiltedTf(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const> const&)185372
mrs_uav_managers::transform_manager::TransformManager::callbackHwApiOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)185372
mrs_uav_managers::transform_manager::TransformManager::onInit()82
mrs_uav_managers::transform_manager::TransformManager::TransformManager()82
mrs_uav_managers::transform_manager::TransformManager::getPrintName[abi:cxx11]() const1329
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..18f625de5c --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.html @@ -0,0 +1,1037 @@ + + + + + + + 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:35442982.5 %
Date:2024-04-09 22:03:33Functions: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          82 :   TransformManager() {
+      50          82 :     ch_ = std::make_shared<estimation_manager::CommonHandlers_t>();
+      51             : 
+      52          82 :     ch_->nodelet_name = nodelet_name_;
+      53          82 :     ch_->package_name = package_name_;
+      54          82 :   }
+      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::Pose 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          82 : void TransformManager::onInit() {
+     150             : 
+     151          82 :   nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+     152             : 
+     153          82 :   ros::Time::waitForValid();
+     154             : 
+     155          82 :   ROS_INFO("[%s]: initializing", getPrintName().c_str());
+     156             : 
+     157          82 :   broadcaster_ = std::make_shared<mrs_lib::TransformBroadcaster>();
+     158             : 
+     159          82 :   ch_->transformer = std::make_shared<mrs_lib::Transformer>(nh_, getPrintName());
+     160          82 :   ch_->transformer->retryLookupNewest(true);
+     161             : 
+     162         164 :   mrs_lib::ParamLoader param_loader(nh_, getPrintName());
+     163             : 
+     164          82 :   param_loader.loadParam("custom_config", _custom_config_);
+     165          82 :   param_loader.loadParam("platform_config", _platform_config_);
+     166          82 :   param_loader.loadParam("world_config", _world_config_);
+     167             : 
+     168          82 :   if (_custom_config_ != "") {
+     169          82 :     param_loader.addYamlFile(_custom_config_);
+     170             :   }
+     171             : 
+     172          82 :   if (_platform_config_ != "") {
+     173          82 :     param_loader.addYamlFile(_platform_config_);
+     174             :   }
+     175             : 
+     176          82 :   if (_world_config_ != "") {
+     177          82 :     param_loader.addYamlFile(_world_config_);
+     178             :   }
+     179             : 
+     180          82 :   param_loader.addYamlFileFromParam("private_config");
+     181          82 :   param_loader.addYamlFileFromParam("public_config");
+     182          82 :   param_loader.addYamlFileFromParam("estimators_config");
+     183             : 
+     184         164 :   const std::string yaml_prefix = "mrs_uav_managers/transform_manager/";
+     185             : 
+     186          82 :   param_loader.loadParam("uav_name", ch_->uav_name);
+     187             : 
+     188             :   /*//{ initialize scope timer */
+     189          82 :   param_loader.loadParam(yaml_prefix + "scope_timer/enabled", ch_->scope_timer.enabled);
+     190         164 :   std::string       filepath;
+     191         164 :   const std::string time_logger_filepath = ros::package::getPath(package_name_) + "/scope_timer/transform_manager_scope_timer.txt";
+     192          82 :   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          82 :   bool   is_origin_param_ok = true;
+     198          82 :   double world_origin_x     = 0;
+     199          82 :   double world_origin_y     = 0;
+     200             : 
+     201          82 :   param_loader.loadParam("world_origin/units", world_origin_units_);
+     202             : 
+     203          82 :   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          82 :   } else if (Support::toLowercase(world_origin_units_) == "latlon") {
+     211             : 
+     212          82 :     ROS_INFO("[%s]: Loading world origin in LatLon units.", getPrintName().c_str());
+     213             : 
+     214             :     double lat, lon;
+     215          82 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_x", lat);
+     216          82 :     is_origin_param_ok &= param_loader.loadParam("world_origin/origin_y", lon);
+     217             : 
+     218          82 :     mrs_lib::UTM(lat, lon, &world_origin_x, &world_origin_y);
+     219             : 
+     220          82 :     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          82 :   world_origin_.x = world_origin_x;
+     228          82 :   world_origin_.y = world_origin_y;
+     229          82 :   world_origin_.z = 0;
+     230             : 
+     231          82 :   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         164 :   std::string local_origin_parent_frame_id;
+     240          82 :   param_loader.loadParam(yaml_prefix + "local_origin_tf/parent", local_origin_parent_frame_id);
+     241          82 :   ns_local_origin_parent_frame_id_ = ch_->uav_name + "/" + local_origin_parent_frame_id;
+     242             : 
+     243         164 :   std::string local_origin_child_frame_id;
+     244          82 :   param_loader.loadParam(yaml_prefix + "local_origin_tf/child", local_origin_child_frame_id);
+     245          82 :   ns_local_origin_child_frame_id_ = ch_->uav_name + "/" + local_origin_child_frame_id;
+     246             : 
+     247          82 :   param_loader.loadParam(yaml_prefix + "local_origin_tf/enabled", publish_local_origin_tf_);
+     248             :   /*//}*/
+     249             : 
+     250             :   /*//{ load stable_origin parameters */
+     251         164 :   std::string stable_origin_parent_frame_id;
+     252          82 :   param_loader.loadParam(yaml_prefix + "stable_origin_tf/parent", stable_origin_parent_frame_id);
+     253          82 :   ns_stable_origin_parent_frame_id_ = ch_->uav_name + "/" + stable_origin_parent_frame_id;
+     254             : 
+     255         164 :   std::string stable_origin_child_frame_id;
+     256          82 :   param_loader.loadParam(yaml_prefix + "stable_origin_tf/child", stable_origin_child_frame_id);
+     257          82 :   ns_stable_origin_child_frame_id_ = ch_->uav_name + "/" + stable_origin_child_frame_id;
+     258             : 
+     259          82 :   param_loader.loadParam(yaml_prefix + "stable_origin_tf/enabled", publish_stable_origin_tf_);
+     260             :   /*//}*/
+     261             : 
+     262             :   /*//{ load fixed_origin parameters */
+     263         164 :   std::string fixed_origin_parent_frame_id;
+     264          82 :   param_loader.loadParam(yaml_prefix + "fixed_origin_tf/parent", fixed_origin_parent_frame_id);
+     265          82 :   ns_fixed_origin_parent_frame_id_ = ch_->uav_name + "/" + fixed_origin_parent_frame_id;
+     266             : 
+     267         164 :   std::string fixed_origin_child_frame_id;
+     268          82 :   param_loader.loadParam(yaml_prefix + "fixed_origin_tf/child", fixed_origin_child_frame_id);
+     269          82 :   ns_fixed_origin_child_frame_id_ = ch_->uav_name + "/" + fixed_origin_child_frame_id;
+     270             : 
+     271          82 :   param_loader.loadParam(yaml_prefix + "fixed_origin_tf/enabled", publish_fixed_origin_tf_);
+     272             :   /*//}*/
+     273             : 
+     274             :   /*//{ load fcu_untilted parameters */
+     275         164 :   std::string fcu_frame_id;
+     276          82 :   param_loader.loadParam(yaml_prefix + "fcu_untilted_tf/parent", fcu_frame_id);
+     277          82 :   ch_->frames.fcu    = fcu_frame_id;
+     278          82 :   ch_->frames.ns_fcu = ch_->uav_name + "/" + fcu_frame_id;
+     279             : 
+     280         164 :   std::string fcu_untilted_frame_id;
+     281          82 :   param_loader.loadParam(yaml_prefix + "fcu_untilted_tf/child", fcu_untilted_frame_id);
+     282          82 :   ch_->frames.fcu_untilted    = fcu_untilted_frame_id;
+     283          82 :   ch_->frames.ns_fcu_untilted = ch_->uav_name + "/" + fcu_untilted_frame_id;
+     284             : 
+     285          82 :   param_loader.loadParam(yaml_prefix + "fcu_untilted_tf/enabled", publish_fcu_untilted_tf_);
+     286             :   /*//}*/
+     287             : 
+     288             :   /*//{ load amsl_origin parameters*/
+     289         164 :   std::string amsl_parent_frame_id, amsl_child_frame_id;
+     290          82 :   param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/enabled", publish_amsl_origin_tf_);
+     291          82 :   param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/parent", amsl_parent_frame_id);
+     292          82 :   param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/child", amsl_child_frame_id);
+     293          82 :   ch_->frames.amsl                = amsl_child_frame_id;
+     294          82 :   ch_->frames.ns_amsl             = ch_->uav_name + "/" + amsl_child_frame_id;
+     295          82 :   ns_amsl_origin_parent_frame_id_ = ch_->uav_name + "/" + amsl_parent_frame_id;
+     296          82 :   ns_amsl_origin_child_frame_id_  = ch_->uav_name + "/" + amsl_child_frame_id;
+     297             : 
+     298             :   /*//}*/
+     299             : 
+     300          82 :   param_loader.loadParam(yaml_prefix + "rtk_antenna/frame_id", ch_->frames.rtk_antenna);
+     301          82 :   ch_->frames.ns_rtk_antenna = ch_->uav_name + "/" + ch_->frames.rtk_antenna;
+     302             : 
+     303          82 :   param_loader.loadParam("mrs_uav_managers/estimation_manager/state_estimators", estimator_names_);
+     304          82 :   param_loader.loadParam(yaml_prefix + "tf_sources", tf_source_names_);
+     305             : 
+     306          82 :   param_loader.loadParam(yaml_prefix + "utm_source_priority", utm_source_priority_list_);
+     307         257 :   for (auto utm_source : utm_source_priority_list_) {
+     308         256 :     if (Support::isStringInVector(utm_source, estimator_names_)) {
+     309          81 :       ROS_INFO("[%s]: the source for utm_origin and world origin is: %s", getPrintName().c_str(), utm_source.c_str());
+     310          81 :       utm_source_name_ = utm_source;
+     311          81 :       break;
+     312             :     }
+     313             :   }
+     314             : 
+     315             :   /*//{ initialize tf sources */
+     316          82 :   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         168 :   for (int i = 0; i < int(estimator_names_.size()); i++) {
+     346             : 
+     347         172 :     const std::string estimator_name = estimator_names_[i];
+     348          86 :     const bool        is_utm_source  = estimator_name == utm_source_name_;
+     349          86 :     ROS_INFO("[%s]: loading tf source of estimator: %s", getPrintName().c_str(), estimator_name.c_str());
+     350             : 
+     351          86 :     auto estimator_param_loader = std::make_shared<mrs_lib::ParamLoader>(nh_, "TransformManager/" + estimator_name);
+     352             : 
+     353          86 :     if (_custom_config_ != "") {
+     354          86 :       estimator_param_loader->addYamlFile(_custom_config_);
+     355             :     }
+     356             : 
+     357          86 :     if (_platform_config_ != "") {
+     358          86 :       estimator_param_loader->addYamlFile(_platform_config_);
+     359             :     }
+     360             : 
+     361          86 :     if (_world_config_ != "") {
+     362          86 :       estimator_param_loader->addYamlFile(_world_config_);
+     363             :     }
+     364             : 
+     365          86 :     estimator_param_loader->addYamlFileFromParam("private_config");
+     366          86 :     estimator_param_loader->addYamlFileFromParam("public_config");
+     367          86 :     estimator_param_loader->addYamlFileFromParam("estimators_config");
+     368             : 
+     369          86 :     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          82 :   param_loader.loadParam(yaml_prefix + "mapping_origin_tf/enabled", mapping_origin_tf_enabled, false);
+     375             : 
+     376          82 :   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         164 :   mrs_lib::SubscribeHandlerOptions shopts;
+     403          82 :   shopts.nh                 = nh_;
+     404          82 :   shopts.node_name          = getPrintName();
+     405          82 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     406          82 :   shopts.threadsafe         = true;
+     407          82 :   shopts.autostart          = true;
+     408          82 :   shopts.queue_size         = 10;
+     409          82 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     410             : 
+     411          82 :   sh_uav_state_ = mrs_lib::SubscribeHandler<mrs_msgs::UavState>(shopts, "uav_state_in", &TransformManager::callbackUavState, this);
+     412             : 
+     413          82 :   sh_height_agl_ = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "height_agl_in", &TransformManager::callbackHeightAgl, this);
+     414             : 
+     415          82 :   sh_altitude_amsl_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in", &TransformManager::callbackAltitudeAmsl, this);
+     416             : 
+     417             :   sh_hw_api_orientation_ =
+     418          82 :       mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "orientation_in", &TransformManager::callbackHwApiOrientation, this);
+     419             : 
+     420          82 :   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          79 :     sh_gnss_ = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, "gnss_in", &TransformManager::callbackGnss, this);
+     424             :   }
+     425             :   /*//}*/
+     426             : 
+     427          82 :   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          82 :   is_initialized_ = true;
+     433          82 :   ROS_INFO("[%s]: initialized", getPrintName().c_str());
+     434          82 : }
+     435             : /*//}*/
+     436             : 
+     437             : /*//{ callbackUavState() */
+     438             : 
+     439      145673 : void TransformManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) {
+     440             : 
+     441      145673 :   if (!is_initialized_) {
+     442        1057 :     return;
+     443             :   }
+     444             : 
+     445      291346 :   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      145673 :   if (!is_first_frame_id_set_) {
+     449          82 :     first_frame_id_                = msg->header.frame_id;
+     450          82 :     last_frame_id_                 = msg->header.frame_id;
+     451          82 :     pose_fixed_                    = msg->pose;
+     452          82 :     pose_first_                    = msg->pose;
+     453          82 :     pose_fixed_diff_.orientation.w = 1;
+     454             : 
+     455          82 :     is_first_frame_id_set_ = true;
+     456             :   }
+     457             : 
+     458             :   // publish static tf from fixed_origin to local_origin based on the first message
+     459      145673 :   if (publish_local_origin_tf_ && !is_local_static_tf_published_) {
+     460          82 :     publishLocalTf();
+     461             :   }
+     462             : 
+     463      145673 :   if (publish_stable_origin_tf_) {
+     464             :     /*//{ publish stable_origin tf*/
+     465      145673 :     geometry_msgs::TransformStamped tf_msg;
+     466      145673 :     tf_msg.header.stamp    = msg->header.stamp;
+     467      145673 :     tf_msg.header.frame_id = ns_stable_origin_parent_frame_id_;
+     468      145673 :     tf_msg.child_frame_id  = ns_stable_origin_child_frame_id_;
+     469             : 
+     470             :     // transform pose to first frame_id
+     471      145673 :     geometry_msgs::PoseStamped pose;
+     472      145673 :     pose.header = msg->header;
+     473      145673 :     pose.pose   = msg->pose;
+     474      145673 :     if (pose.pose.orientation.w == 0 && pose.pose.orientation.z == 0 && pose.pose.orientation.y == 0 && pose.pose.orientation.x == 0) {
+     475           1 :       ROS_WARN_ONCE("[%s]: Uninitialized quaternion detected during publishing stable_origin tf of %s. Setting w=1", getPrintName().c_str(),
+     476             :                     pose.header.frame_id.c_str());
+     477           1 :       pose.pose.orientation.w = 1.0;
+     478             :     }
+     479             : 
+     480      145673 :     auto res = ch_->transformer->transformSingle(pose, first_frame_id_);
+     481             : 
+     482      145673 :     if (res) {
+     483      144616 :       const tf2::Transform      tf       = Support::tf2FromPose(res->pose);
+     484      144616 :       const tf2::Transform      tf_inv   = tf.inverse();
+     485      144616 :       const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     486      144616 :       tf_msg.transform.translation       = Support::pointToVector3(pose_inv.position);
+     487      144616 :       tf_msg.transform.rotation          = pose_inv.orientation;
+     488             : 
+     489      144616 :       if (Support::noNans(tf_msg)) {
+     490             :         try {
+     491      144616 :           broadcaster_->sendTransform(tf_msg);
+     492             :         }
+     493           0 :         catch (...) {
+     494           0 :           ROS_ERROR("exception caught ");
+     495             :         }
+     496             :       } else {
+     497           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(),
+     498             :                           tf_msg.child_frame_id.c_str());
+     499             :       }
+     500      144616 :       ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     501             :                     tf_msg.child_frame_id.c_str());
+     502             :     } else {
+     503        1057 :       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());
+     504        1057 :       return;
+     505             :     }
+     506             :     /*//}*/
+     507             :   }
+     508             : 
+     509      144616 :   if (publish_fixed_origin_tf_) {
+     510             :     /*//{ publish fixed_origin tf*/
+     511      144616 :     if (msg->header.frame_id != last_frame_id_) {
+     512           4 :       ROS_WARN("[%s]: Detected estimator change from %s to %s. Updating offset for fixed origin.", getPrintName().c_str(), last_frame_id_.c_str(),
+     513             :                msg->header.frame_id.c_str());
+     514             : 
+     515           4 :       pose_fixed_diff_ = Support::getPoseDiff(msg->pose, pose_fixed_);
+     516             :     }
+     517             : 
+     518             : 
+     519      144616 :     pose_fixed_ = Support::applyPoseDiff(msg->pose, pose_fixed_diff_);
+     520             : 
+     521      289232 :     geometry_msgs::TransformStamped tf_msg;
+     522      144616 :     tf_msg.header.stamp    = msg->header.stamp;
+     523      144616 :     tf_msg.header.frame_id = ns_fixed_origin_parent_frame_id_;
+     524      144616 :     tf_msg.child_frame_id  = ns_fixed_origin_child_frame_id_;
+     525             : 
+     526      144616 :     const tf2::Transform      tf       = Support::tf2FromPose(pose_fixed_);
+     527      144616 :     const tf2::Transform      tf_inv   = tf.inverse();
+     528      144616 :     const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv);
+     529      144616 :     tf_msg.transform.translation       = Support::pointToVector3(pose_inv.position);
+     530      144616 :     tf_msg.transform.rotation          = pose_inv.orientation;
+     531             : 
+     532      144616 :     if (Support::noNans(tf_msg)) {
+     533             :       try {
+     534      144616 :         broadcaster_->sendTransform(tf_msg);
+     535             :       }
+     536           0 :       catch (...) {
+     537           0 :         ROS_ERROR("exception caught ");
+     538             :       }
+     539             :     } else {
+     540           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(),
+     541             :                         tf_msg.child_frame_id.c_str());
+     542             :     }
+     543      144616 :     ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     544             :                   tf_msg.child_frame_id.c_str());
+     545             :     /*//}*/
+     546             :   }
+     547             : 
+     548             :   /*//{ choose another source of utm and world tfs after estimator switch */
+     549      144616 :   if (msg->header.frame_id != last_frame_id_) {
+     550             : 
+     551           8 :     const std::string last_estimator_name    = Support::frameIdToEstimatorName(last_frame_id_);
+     552           8 :     const std::string current_estimator_name = Support::frameIdToEstimatorName(msg->header.frame_id);
+     553             : 
+     554           4 :     ROS_INFO("[%s]: Detected estimator switch: %s -> %s", getPrintName().c_str(), last_estimator_name.c_str(), current_estimator_name.c_str());
+     555             : 
+     556           4 :     bool   valid_utm_source_found = false;
+     557             :     size_t potential_utm_source_index;
+     558             : 
+     559          10 :     for (size_t i = 0; i < tf_sources_.size(); i++) {
+     560             : 
+     561             :       // first check if tf source can publish utm origin and is not the switched-from estimator
+     562          10 :       if (tf_sources_.at(i)->getIsUtmBased() && tf_sources_.at(i)->getName() != last_estimator_name) {
+     563             : 
+     564           7 :         valid_utm_source_found     = true;
+     565           7 :         potential_utm_source_index = i;
+     566             : 
+     567             :         // check if switched-to estimator is utm_based, if so, use it
+     568           7 :         if (tf_sources_.at(i)->getIsUtmBased() && tf_sources_.at(i)->getName() == current_estimator_name) {
+     569           4 :           potential_utm_source_index = i;
+     570           4 :           break;
+     571             :         }
+     572             :       }
+     573             :     }
+     574             : 
+     575             : 
+     576             :     // if we found a valid utm source, use it, otherwise stay with the switched-from estimator
+     577           4 :     if (valid_utm_source_found) {
+     578             : 
+     579             :       // stop all estimators from publishing utm source
+     580          20 :       for (size_t i = 0; i < tf_sources_.size(); i++) {
+     581          16 :         if (tf_sources_.at(i)->getIsUtmSource()) {
+     582           4 :           tf_sources_.at(i)->setIsUtmSource(false);
+     583           4 :           ROS_INFO("[%s]: setting is_utm_source of estimator %s to false", getPrintName().c_str(), last_estimator_name.c_str());
+     584             :         }
+     585             :       }
+     586             : 
+     587           4 :       tf_sources_.at(potential_utm_source_index)->setIsUtmSource(true);
+     588           4 :       ROS_INFO("[%s]: setting is_utm_source of estimator %s to true", getPrintName().c_str(), current_estimator_name.c_str());
+     589             :     }
+     590             :   }
+     591             :   /*//}*/
+     592             : 
+     593      144616 :   last_frame_id_ = msg->header.frame_id;
+     594             : }
+     595             : /*//}*/
+     596             : 
+     597             : /*//{ callbackHeightAgl() */
+     598             : 
+     599      128083 : void TransformManager::callbackHeightAgl(const mrs_msgs::Float64Stamped::ConstPtr msg) {
+     600             : 
+     601      128083 :   if (!is_initialized_) {
+     602           0 :     return;
+     603             :   }
+     604             : 
+     605      384249 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackHeightAgl", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     606             : 
+     607      256166 :   geometry_msgs::TransformStamped tf_msg;
+     608      128083 :   tf_msg.header.stamp    = msg->header.stamp;
+     609      128083 :   tf_msg.header.frame_id = ch_->frames.ns_fcu_untilted;
+     610      128083 :   tf_msg.child_frame_id  = msg->header.frame_id;
+     611             : 
+     612      128083 :   tf_msg.transform.translation.x = 0;
+     613      128083 :   tf_msg.transform.translation.y = 0;
+     614      128083 :   tf_msg.transform.translation.z = -msg->value;
+     615      128083 :   tf_msg.transform.rotation.x    = 0;
+     616      128083 :   tf_msg.transform.rotation.y    = 0;
+     617      128083 :   tf_msg.transform.rotation.z    = 0;
+     618      128083 :   tf_msg.transform.rotation.w    = 1;
+     619             : 
+     620      128083 :   if (Support::noNans(tf_msg)) {
+     621             :     try {
+     622      128083 :       broadcaster_->sendTransform(tf_msg);
+     623             :     }
+     624           0 :     catch (...) {
+     625           0 :       ROS_ERROR("exception caught ");
+     626             :     }
+     627             :   } else {
+     628           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(),
+     629             :                       tf_msg.child_frame_id.c_str());
+     630             :   }
+     631      128083 :   ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     632             :                 tf_msg.child_frame_id.c_str());
+     633             : }
+     634             : /*//}*/
+     635             : 
+     636             : /*//{ callbackAltitudeAmsl() */
+     637             : 
+     638      146902 : void TransformManager::callbackAltitudeAmsl(const mrs_msgs::HwApiAltitude::ConstPtr msg) {
+     639             : 
+     640      146902 :   if (!is_initialized_) {
+     641           0 :     return;
+     642             :   }
+     643             : 
+     644      440706 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackAltitudeAmsl", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     645             : 
+     646      293804 :   geometry_msgs::TransformStamped tf_msg;
+     647      146902 :   tf_msg.header.stamp    = msg->stamp;
+     648      146902 :   tf_msg.header.frame_id = ch_->frames.ns_fcu_untilted;
+     649      146902 :   tf_msg.child_frame_id  = ch_->frames.ns_amsl;
+     650             : 
+     651      146902 :   tf_msg.transform.translation.x = 0;
+     652      146902 :   tf_msg.transform.translation.y = 0;
+     653      146902 :   tf_msg.transform.translation.z = -msg->amsl;
+     654      146902 :   tf_msg.transform.rotation.x    = 0;
+     655      146902 :   tf_msg.transform.rotation.y    = 0;
+     656      146902 :   tf_msg.transform.rotation.z    = 0;
+     657      146902 :   tf_msg.transform.rotation.w    = 1;
+     658             : 
+     659      146902 :   if (Support::noNans(tf_msg)) {
+     660             :     try {
+     661      146861 :       broadcaster_->sendTransform(tf_msg);
+     662             :     }
+     663           0 :     catch (...) {
+     664           0 :       ROS_ERROR("exception caught ");
+     665             :     }
+     666             :   } else {
+     667          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(),
+     668             :                       tf_msg.child_frame_id.c_str());
+     669             :   }
+     670      146902 :   ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     671             :                 tf_msg.child_frame_id.c_str());
+     672             : }
+     673             : /*//}*/
+     674             : 
+     675             : /*//{ callbackHwApiOrientation() */
+     676      185372 : void TransformManager::callbackHwApiOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     677             : 
+     678      185372 :   if (!is_initialized_) {
+     679           0 :     return;
+     680             :   }
+     681             : 
+     682      556116 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::publishFcuUntilted", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     683             : 
+     684      185372 :   if (publish_fcu_untilted_tf_) {
+     685      185372 :     publishFcuUntiltedTf(msg);
+     686             :   }
+     687             : }
+     688             : /*//}*/
+     689             : 
+     690             : /*//{ callbackGnss() */
+     691       87120 : void TransformManager::callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg) {
+     692             : 
+     693       87120 :   if (!is_initialized_) {
+     694       87041 :     return;
+     695             :   }
+     696             : 
+     697       87120 :   if (got_utm_offset_) {
+     698       87041 :     return;
+     699             :   }
+     700             : 
+     701         158 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackGnss", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     702             : 
+     703             :   double out_x;
+     704             :   double out_y;
+     705             : 
+     706          79 :   mrs_lib::UTM(msg->latitude, msg->longitude, &out_x, &out_y);
+     707             : 
+     708          79 :   if (!std::isfinite(out_x)) {
+     709           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_x\"!!!", getPrintName().c_str());
+     710           0 :     return;
+     711             :   }
+     712             : 
+     713          79 :   if (!std::isfinite(out_y)) {
+     714           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_y\"!!!", getPrintName().c_str());
+     715           0 :     return;
+     716             :   }
+     717             : 
+     718          79 :   geometry_msgs::Point utm_origin;
+     719          79 :   utm_origin.x = out_x;
+     720          79 :   utm_origin.y = out_y;
+     721          79 :   utm_origin.z = msg->altitude;
+     722             : 
+     723          79 :   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);
+     724             : 
+     725         159 :   for (size_t i = 0; i < tf_sources_.size(); i++) {
+     726          80 :     tf_sources_[i]->setUtmOrigin(utm_origin);
+     727          80 :     tf_sources_[i]->setWorldOrigin(world_origin_);
+     728             :   }
+     729          79 :   got_utm_offset_ = true;
+     730             : }
+     731             : /*//}*/
+     732             : 
+     733             : /*//{ callbackRtkGps() */
+     734        3792 : void TransformManager::callbackRtkGps(const mrs_msgs::RtkGps::ConstPtr msg) {
+     735             : 
+     736        3792 :   if (!is_initialized_) {
+     737        3789 :     return;
+     738             :   }
+     739             : 
+     740        3792 :   if (got_utm_offset_) {
+     741        3787 :     return;
+     742             :   }
+     743             : 
+     744          10 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackRtkGps", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     745             : 
+     746             :   double out_x;
+     747             :   double out_y;
+     748             : 
+     749           5 :   geometry_msgs::PoseStamped rtk_pos;
+     750             : 
+     751           5 :   if (!std::isfinite(msg->gps.latitude)) {
+     752           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->latitude\"!!!", getPrintName().c_str());
+     753           0 :     return;
+     754             :   }
+     755             : 
+     756           5 :   if (!std::isfinite(msg->gps.longitude)) {
+     757           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->longitude\"!!!", getPrintName().c_str());
+     758           0 :     return;
+     759             :   }
+     760             : 
+     761           5 :   if (!std::isfinite(msg->gps.altitude)) {
+     762           0 :     ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in RTK variable \"msg->altitude\"!!!", getPrintName().c_str());
+     763           0 :     return;
+     764             :   }
+     765             : 
+     766           5 :   if (msg->fix_type.fix_type != mrs_msgs::RtkFixType::RTK_FIX) {
+     767           0 :     ROS_INFO_THROTTLE(1.0, "[%s] %s RTK FIX", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     768           0 :     return;
+     769             :   }
+     770             : 
+     771           5 :   rtk_pos.header = msg->header;
+     772           5 :   mrs_lib::UTM(msg->gps.latitude, msg->gps.longitude, &rtk_pos.pose.position.x, &rtk_pos.pose.position.y);
+     773           5 :   rtk_pos.pose.position.z  = msg->gps.altitude;
+     774           5 :   rtk_pos.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+     775             : 
+     776           5 :   rtk_pos.pose.position.x -= ch_->world_origin.x;
+     777           5 :   rtk_pos.pose.position.y -= ch_->world_origin.y;
+     778             : 
+     779             : 
+     780             :   // transform the RTK position from antenna to FCU
+     781           5 :   auto res = transformRtkToFcu(rtk_pos);
+     782           5 :   if (res) {
+     783           3 :     rtk_pos.pose = res.value();
+     784             :   } else {
+     785           2 :     ROS_ERROR_THROTTLE(1.0, "[%s]: transform to fcu failed", getPrintName().c_str());
+     786           2 :     return;
+     787             :   }
+     788             : 
+     789           3 :   mrs_lib::UTM(msg->gps.latitude, msg->gps.longitude, &out_x, &out_y);
+     790             : 
+     791           3 :   if (!std::isfinite(out_x)) {
+     792           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_x\"!!!", getPrintName().c_str());
+     793           0 :     return;
+     794             :   }
+     795             : 
+     796           3 :   if (!std::isfinite(out_y)) {
+     797           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_y\"!!!", getPrintName().c_str());
+     798           0 :     return;
+     799             :   }
+     800             : 
+     801           3 :   geometry_msgs::Point utm_origin;
+     802           3 :   utm_origin.x = out_x;
+     803           3 :   utm_origin.y = out_y;
+     804           3 :   utm_origin.z = msg->gps.altitude;
+     805             : 
+     806           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);
+     807             : 
+     808           9 :   for (size_t i = 0; i < tf_sources_.size(); i++) {
+     809           6 :     tf_sources_[i]->setUtmOrigin(utm_origin);
+     810           6 :     tf_sources_[i]->setWorldOrigin(world_origin_);
+     811             :   }
+     812           3 :   got_utm_offset_ = true;
+     813             : }
+     814             : /*//}*/
+     815             : 
+     816             : /*//{ publishFcuUntiltedTf() */
+     817      185372 : void TransformManager::publishFcuUntiltedTf(const geometry_msgs::QuaternionStampedConstPtr& msg) {
+     818             : 
+     819      370744 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::publishFcuUntilted", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     820             : 
+     821             :   double heading;
+     822             : 
+     823             :   try {
+     824      185372 :     heading = mrs_lib::AttitudeConverter(msg->quaternion).getHeading();
+     825             :   }
+     826           0 :   catch (...) {
+     827           0 :     ROS_ERROR("[%s]: Exception caught during getting heading", getPrintName().c_str());
+     828           0 :     return;
+     829             :   }
+     830      185372 :   scope_timer.checkpoint("heading");
+     831             : 
+     832      185372 :   const Eigen::Matrix3d odom_pixhawk_R = mrs_lib::AttitudeConverter(msg->quaternion);
+     833      185372 :   const Eigen::Matrix3d undo_heading_R = mrs_lib::AttitudeConverter(Eigen::AngleAxis(-heading, Eigen::Vector3d(0, 0, 1)));
+     834             : 
+     835      185372 :   const tf2::Quaternion q     = mrs_lib::AttitudeConverter(undo_heading_R * odom_pixhawk_R);
+     836      185372 :   const tf2::Quaternion q_inv = q.inverse();
+     837             : 
+     838      185372 :   scope_timer.checkpoint("q inverse");
+     839             : 
+     840      185372 :   geometry_msgs::TransformStamped tf;
+     841      185372 :   tf.header.stamp            = msg->header.stamp;  // TODO(petrlmat) ros::Time::now()?
+     842      185372 :   tf.header.frame_id         = ch_->frames.ns_fcu;
+     843      185372 :   tf.child_frame_id          = ch_->frames.ns_fcu_untilted;
+     844      185372 :   tf.transform.translation.x = 0.0;
+     845      185372 :   tf.transform.translation.y = 0.0;
+     846      185372 :   tf.transform.translation.z = 0.0;
+     847      185372 :   tf.transform.rotation      = mrs_lib::AttitudeConverter(q_inv);
+     848             : 
+     849      185372 :   scope_timer.checkpoint("tf fill");
+     850      185372 :   if (Support::noNans(tf)) {
+     851      185372 :     broadcaster_->sendTransform(tf);
+     852             :   } else {
+     853           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaN encountered in fcu_untilted tf", getPrintName().c_str());
+     854             :   }
+     855      185372 :   scope_timer.checkpoint("tf pub");
+     856             : }
+     857             : /*//}*/
+     858             : 
+     859             : /* publishLocalTf() //{*/
+     860          82 : void TransformManager::publishLocalTf() {
+     861             : 
+     862         246 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishLocalTf", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     863             : 
+     864          82 :   geometry_msgs::TransformStamped tf_msg;
+     865          82 :   tf_msg.header.stamp = ros::Time::now();
+     866             : 
+     867          82 :   tf_msg.header.frame_id       = ns_fixed_origin_child_frame_id_;
+     868          82 :   tf_msg.child_frame_id        = ns_local_origin_child_frame_id_;
+     869          82 :   tf_msg.transform.translation = Support::pointToVector3(pose_first_.position);
+     870          82 :   tf_msg.transform.rotation    = pose_first_.orientation;
+     871             : 
+     872          82 :   if (Support::noNans(tf_msg)) {
+     873             : 
+     874             :     try {
+     875          82 :       static_broadcaster_.sendTransform(tf_msg);
+     876             :     }
+     877           0 :     catch (...) {
+     878           0 :       ROS_ERROR("[%s]: exception caught while publishing tf from %s to %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     879             :                 tf_msg.child_frame_id.c_str());
+     880             :     }
+     881             : 
+     882             :   } else {
+     883           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(),
+     884             :                       tf_msg.child_frame_id.c_str());
+     885             :   }
+     886          82 :   ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(),
+     887             :                 tf_msg.child_frame_id.c_str());
+     888          82 :   is_local_static_tf_published_ = true;
+     889          82 : }
+     890             : /*//}*/
+     891             : 
+     892             : /*//{ transformRtkToFcu() */
+     893           5 : std::optional<geometry_msgs::Pose> TransformManager::transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const {
+     894             : 
+     895          10 :   geometry_msgs::PoseStamped pose_tmp = pose_in;
+     896             : 
+     897             :   // inject current orientation into rtk pose
+     898          10 :   auto res1 = ch_->transformer->getTransform(ch_->frames.ns_fcu_untilted, ch_->frames.ns_fcu, ros::Time::now());
+     899           5 :   if (res1) {
+     900           3 :     pose_tmp.pose.orientation = res1.value().transform.rotation;
+     901             :   } else {
+     902           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(),
+     903             :                        ch_->frames.ns_fcu.c_str());
+     904           2 :     return {};
+     905             :   }
+     906             : 
+     907             :   // invert tf
+     908           3 :   tf2::Transform             tf_utm_to_antenna = Support::tf2FromPose(pose_tmp.pose);
+     909           6 :   geometry_msgs::PoseStamped utm_in_antenna;
+     910           3 :   utm_in_antenna.pose            = Support::poseFromTf2(tf_utm_to_antenna.inverse());
+     911           3 :   utm_in_antenna.header.stamp    = pose_in.header.stamp;
+     912           3 :   utm_in_antenna.header.frame_id = ch_->frames.ns_rtk_antenna;
+     913             : 
+     914             :   // transform to fcu
+     915           6 :   geometry_msgs::PoseStamped utm_in_fcu;
+     916           3 :   utm_in_fcu.header.frame_id = ch_->frames.ns_fcu;
+     917           3 :   utm_in_fcu.header.stamp    = pose_in.header.stamp;
+     918           6 :   auto res2                  = ch_->transformer->transformSingle(utm_in_antenna, ch_->frames.ns_fcu);
+     919             : 
+     920           3 :   if (res2) {
+     921           3 :     utm_in_fcu = res2.value();
+     922             :   } else {
+     923           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(),
+     924             :                        ch_->frames.ns_fcu.c_str());
+     925           0 :     return {};
+     926             :   }
+     927             : 
+     928             :   // invert tf
+     929           3 :   tf2::Transform      tf_fcu_to_utm = Support::tf2FromPose(utm_in_fcu.pose);
+     930           3 :   geometry_msgs::Pose fcu_in_utm    = Support::poseFromTf2(tf_fcu_to_utm.inverse());
+     931             : 
+     932           3 :   return fcu_in_utm;
+     933             : }
+     934             : /*//}*/
+     935             : 
+     936             : /*//{ getName() */
+     937           0 : std::string TransformManager::getName() const {
+     938           0 :   return name_;
+     939             : }
+     940             : /*//}*/
+     941             : 
+     942             : /*//{ getPrintName() */
+     943        1329 : std::string TransformManager::getPrintName() const {
+     944        1329 :   return nodelet_name_;
+     945             : }
+     946             : /*//}*/
+     947             : 
+     948             : }  // namespace transform_manager
+     949             : 
+     950             : }  // namespace mrs_uav_managers
+     951             : 
+     952             : #include <pluginlib/class_list_macros.h>
+     953          82 : 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..30f0c970f4 --- /dev/null +++ b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.overview.html @@ -0,0 +1,259 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_managers/src/transform_manager/transform_manager.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.png b/mrs_uav_managers/src/transform_manager/transform_manager.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..18c8bb7bac38a7222ed4e36837a4cf8f66d14032 GIT binary patch literal 3373 zcmV+|4bt+7P) ze7~L5Uer9vdy8ZN8o-^bLz}HYk^cyg=T3pKt4M{$^5q1YU#8Q9I5Yi1w1CM5l!2ibR-xfu+O-t1)_w;A8@@sYp0XSya*-%_9p`bN{|R`(5BKq0 zyIu)g7AOy7DQ5Ud)5Ei7cy|nj_Hk5}g;RsQ3}pLAfzr%?k?Vat0=#Mptw4@@J%xi^ z1T!W#P{cccCUF3>|)m`gFLpd5XYMktZtWSXMxhX-Am|rb7Z>>BEqCutXS%^Mfh+RD{$bQadD>ozucQhjV`_|vG5J*!!ej;mx$7kUz-HnW{5FZx*l-oJz$f8eU*WIxS}xK@ z87THKQ>cJRft|w+-H;Q9LwLygI(H^zZtZi;hNL}%u?TW-u0K*AW!8!JJ6+d*M~;8* z^{I2M^uapUB{*|ABx_P+v;hJ81P&+b*kA;u*kdVyG@&Tz&R}T&4((C5qbq@f$F9Mk zWzRfn4Op)|sWa_9Is_z>SPw>+ZC3$hidLmK-`i`*F+UTauz+O*2WCnfo z35wVIvC=geg=&3?q33sr+g_1d$p?XWECKiD_`?KzmWVq5jZqxQNj*~3h_XUmiJ_fh z>HPyHwn}?+QcP6#9zGP{7t4)?y)U3G%C##G=%d<6wU6Hzcwry+w4+E~>*JFEFX*G% z0BaTZv;s5qP}WswkC^4UAeT}+=6PSK=HfpioL{6~>yx8fIAoRTODp2d53u^V8n{2l zA10tH1}FhD!H`!^mIhg>zCJ=LDHH|tQ8ew)LR-jiL_n*Fv$n;!L#y`@=bA6RH^3N`c|{4Tw9>C##aoqRVL4uoW zE^`eAqZLcON$ABNDrMCcD+Rx1H5zUHoy zwi;i|IkNUosqXaQ^F=uQKvN2oweg*)7JyjcoZkM`M>g^TKu-gj3!5>iSd$*N#n+H? zkMWGr-RoIWtdY;@sTA_YXy-JMbzSB311@ZK5@l3^kY+nf^6H|j{Vb%pCW{D@bg_@R zDr%rwmvi=KTZN83M!AvxfmJxm(bmTw_j`C??e`o#0nxH>(Zrc790n>Du9hRqdh;tq zHA*(3IkZ*jo4O5XuyD~9R*eVRasWmP_eJcCXyH82lufyr7ki_H!#h5Th4ZYt-s>=Z zMb)T<13oU9n`$9_qxrby3s+LOIfW=BUBNQNGApA|7^+pks}*_+DUe&Oj-I|shAT|m z!6NMip1+K%zQ`JPou_DS3t#KDGPQ-q!Q4Vj8Zr%D0YWKD&=jA22+5^|Emx#jh_&ma z9eQ2DdTRilknhRE2KV^QX*=D&7jqK0ZP-lo!FMtPn|<8>u@-4t*NS&tMRME49daIN z#YxLgJP;b6tMtS8T_m1&QsbOso9g>77vtL{DMe-ArI54^d>{~|#nz~8e#wf9)Y(d5 zY{F2gVUbvY8%o-~I={I1vSN23yIuNd-k-VX^ku5;IpsP#et)L^@Phpr@46X0$O|{^ z&fA|c266Z#i4w$NinKUO0y<53j}Jz$!b3oQ*z~N_Vx;)9Kx6CLi?f7TC*+@GYLLOqV6)jSW<-NMuBkE`R1j zH6W5zpmyQUDws#Dj-2j&C={)QKp&xI4G)D=vr6W_u(q%i7wfElSN)^)@7S1u{eyuD zmNQ;Z;Ksq=_gcAo7KJhX(!yBt{K&P>-kiR7+51(40Skw+lqTTrh^lYTA>gLHxcz>( z%i~E;`BcLG&~-|~v?;|DH3(x8fVJQ|{^D0+)+)&Vj6AMx<5*7$FQ#7levlfD`H(h8z{^E+ zk$Kv`K|rUL3?iu!1zF2!)niFOaQjGICuIj(S=(DGOGtmpJd`WU)zZqH84yNoItozy zp7vb#o%{rPkhbAVIh1doVKhrO%To!b{z9qWFqkqD@r7A_B9s#0f1#9d)c-~(rAJd& zY*k_!r-cA#%EQ_dZYwHw(;|l4k za16l=Ji^*>SBJzICamQnr3bfsm6N9v)bNP(y6a%`AW5gBKZ;P>hSJy z23jem8-ecmWYeC-ViaHrVI}*@=IPqJgS0j_g>oP1RO42FBgv~ZA0TmCef*N1JZHn^ zwSDvbRTuqOj%O@LUS-dVY8#_HobF(w8E6U?iK$ksr;ku%I|l`Bm1E4h)@fEjJaE)3 z#mZ~uMnZw|m;(^_-tu*No%Xup< + + + + + + 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:746118063.2 %
Date:2024-04-09 22:03:33Functions:353989.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_managers::uav_manager::UavManager::pirouetteSrv()0
mrs_uav_managers::uav_manager::UavManager::callbackMinHeightCheck(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_managers::uav_manager::UavManager::elandSrv()0
mrs_uav_managers::uav_manager::UavManager::ehoverSrv()0
mrs_uav_managers::uav_manager::UavManager::callbackLandHome(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)1
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::callbackLand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)2
mrs_uav_managers::uav_manager::UavManager::landSrv()4
mrs_uav_managers::uav_manager::UavManager::landImpl[abi:cxx11]()4
mrs_uav_managers::uav_manager::UavManager::ungripSrv()5
mrs_uav_managers::uav_manager::UavManager::disarmSrv()6
mrs_uav_managers::uav_manager::UavManager::landWithDescendImpl[abi:cxx11]()7
mrs_uav_managers::uav_manager::UavManager::changeLandingState(mrs_uav_managers::uav_manager::LandingStates_t)13
mrs_uav_managers::uav_manager::UavManager::takeoffSrv()19
mrs_uav_managers::uav_manager::UavManager::callbackTakeoff(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)21
mrs_uav_managers::uav_manager::UavManager::offboardSrv(bool)56
mrs_uav_managers::uav_manager::UavManager::midairActivationImpl[abi:cxx11]()57
mrs_uav_managers::uav_manager::UavManager::timerMidairActivation(ros::TimerEvent const&)57
mrs_uav_managers::uav_manager::UavManager::callbackMidairActivation(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)57
mrs_uav_managers::uav_manager::UavManager::toggleControlOutput(bool const&)60
mrs_uav_managers::uav_manager::UavManager::emergencyReferenceSrv(mrs_msgs::ReferenceStamped_<std::allocator<void> > const&)78
mrs_uav_managers::uav_manager::UavManager::setControlCallbacksSrv(bool const&)79
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_managers::uav_manager::UavManager::initialize()82
mrs_uav_managers::uav_manager::UavManager::preinitialize()82
mrs_uav_managers::uav_manager::UavManager::onInit()82
mrs_uav_managers::uav_manager::UavManager::timerHwApiCapabilities(ros::TimerEvent const&)84
mrs_uav_managers::uav_manager::UavManager::setOdometryCallbacksSrv(bool const&)99
mrs_uav_managers::uav_manager::UavManager::switchControllerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)158
mrs_uav_managers::uav_manager::UavManager::switchTrackerSrv(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)159
mrs_uav_managers::uav_manager::UavManager::timerFlightTime(ros::TimerEvent const&)259
mrs_uav_managers::uav_manager::UavManager::timerLanding(ros::TimerEvent const&)604
mrs_uav_managers::uav_manager::UavManager::timerTakeoff(ros::TimerEvent const&)920
mrs_uav_managers::uav_manager::UavManager::timerDiagnostics(ros::TimerEvent const&)1832
mrs_uav_managers::uav_manager::UavManager::timerMaxHeight(ros::TimerEvent const&)18446
mrs_uav_managers::uav_manager::UavManager::timerMinHeight(ros::TimerEvent const&)18446
mrs_uav_managers::uav_manager::UavManager::timerMaxthrottle(ros::TimerEvent const&)45728
mrs_uav_managers::uav_manager::UavManager::callbackHwApiGNSS(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)87502
mrs_uav_managers::uav_manager::UavManager::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)145636
+
+
+ + + +
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..b0e6b56759 --- /dev/null +++ b/mrs_uav_managers/src/uav_manager.cpp.func.html @@ -0,0 +1,236 @@ + + + + + + + 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:746118063.2 %
Date:2024-04-09 22:03:33Functions:353989.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

*IE5g1r!$o$-9n*W2E98)em&X>Y^{0-nWRQ(8;C+ z9YWBY1-NBLBc{qxTVYCEF1R-?0x$zd#GhDPBy#r7^3X6bt(@w|%=NMsqsHtZEKcpk zm<_U<>dKLmz5>4jBM&IYh*ZT!jKET8rd`alq!NF9m&J9g{n~27E{lul4g1`$TU}?r z4|5+`9^_I60GBiebE?sQb6s<_^fGnb!=9PkNv44DomAiIr7Mvbw>Z!d`(i-4%2XHP z8p2(l^zzO(4>|)7$GA|p05E{XESGnEN{QiWG;t3a_J|*$70bLq;wmr%DAg3gLcsf_3|xh& z%nAYT@2{Sn63=-)3fV(DNTIgymF1o%-JLGT77jR@fLIh+Kya^0zYDP7MqUA70GOzn zbp-=uB910|14bTDfiaUv2D#oPZE4*NE@>$^8!#4?3|$!aj*$X&_Q(sr`s+wJ6}Usx zRou|K0IaI)Uo(4Y+)(k~WryeDSb0lBx@n$nbEYj0?KTGjCQy~K?HKF39dXtVKOOPY z5vMV}&GC_9yh!h$IX$F?m_d7#w<1kFq-7y)(Gp@jJ)}sLzOEVJJ3XXMjKr>KhViH# z5>R_WC3QcthqUQzbG8UK&-Ljc8DRuh)Y&}8T6##pxL*3Ms1tffUwI7*c>oE7sI<=6 z`ph82MF8qBF0~B-15$R>Z>Ud7?B{zbiZ*b?L?1 zN8{wDrt7f9xwE3Fa=$+a*jkA1-Qu`4l?Q(hc9oS?n2XTiWSHw9Q7$?#gca+XBOkE2 ziZ~(^caTUN418n_0m?DP5uwAyy)paZfX@$zVjyj%Q+rV}!EGI*(7wNk_~LT0#MPS+a@t8P2Hch=&tFh5YK6gs}}M#kjZC%)UdfwVK)7 z2nKA0GkP+RcJUr}?d_1Bb>p5h#;e(an9S!Y7mFTaJla%0;?$XdsJwmAf5vBGn3(`; zFK+0&W^Txh-&13h)Jm@yBMFs@fiu9`4Z<%#>GKnBJLHo|c!f!Cm+)#GiEB1u28C^B zF(dZMnFcIr&HxV}X7m~<#(1L?Ta8f=evqF;whut`h#CK& zMYot)RIuSGJjDzHzDvwX4}9xfzctU5c2^=`5RX#!MqOQzX-tlI$$ct?B9kza9J@qp z)fi2yXAcsdtv2o%xZ?QmuFt^bkEe&b3j@N9uZWt|Xi-)(ajazztNF|Bekfbq=ON(H zoU{FNAAwmf3;A%)8ESdg$CzvQF0p^+c(hVdyVa2rBBZ)_N+mrHQ3YFv^n_c*CUpC= z8>3iy0>1|%e#pTxs@NVmi*Hsr5bq-w=y)!0LT4WV$| zwS?k7yaL6%=11}oooUFa{^NXPq_W_nug>DBKKZy10Sl6kQ8Fp2{}vxrp(et%FG`#I zb=HL@O&ZmcNd;XU;AHr^-E+wy9mBzv z#=P;UObx-D(&NEAETK%~z!weqW7Xw)4b{|s&936{SNH1!qygkNh1fAm=uWg(sFqX~^_44E1G&x{cJk#bxHCSkpTotah_#`P&0Ehi z>Y91WvK*tEIh)5z&|TV1KmkTg*Oj|&65!ij5V`m#)5B`zJLp=jGm~_smMaaYs|2K% zEAgPNsGjQoFRp3!-#-_ZsOtkPF89A*UqigIWDFG}qX!&e;m8njv}q%8f-7RTcE8Sd ztEdl3Vg`Q;%jmZ+dIJliPgk}CSoTO#n~y+6VWxnlQBp-|+NK+zo$eVu^TwEKAN-fF zYv{3COkvKTAy^m`;ZpAdsAa5?d*4Gs>dgNHJrGU+hH-#%Wd9W~`G){ddXhvekNCRA z3LiyX-_#otcbpPqoHc+gC52cWXC^C_&{JKzVUXxr3Rpp_U9L%q35CLXzXs0HmKSaI zykevpEr^j}7cNMpza*d}TA&U1%0kGZ=7eI5>f4(P=`HLlX3wWzx7D`H4pTh0``f8r zxu8$;5lh{sm^niOIZNG~(&Yf}Fi{Z$pOw-g>-3daKOV6phOR6iIQ{_CC#v_!_%u0w zz>*lE9Oeg{$k9;!B`G#J!+T0%qP%8hT#=YRs3{6QGVwi{j|2z4jE@IO`K|%d%Htr~ zJeR{|1gPoy3yt9&PYwBHn}0f{G0YtNIE`VK`QWbGx&zhxOojG^{{1r5EN!P#Q3bSL z3%Bh~MS0NXbSh4#0`u`Bor)(=8h=z%oK8g*u#b8VHogPdDr(9AtDwp)E*3^s3xN{FwRa8z;QB!Vh-768 zF~#o6jG&l8AiySF^Rv^5(PpmAA0^Trs(ScnDyyK&Lsj(dp+EQ~JX9q*rnSqVs;|t? z?00!!xquH_tVGuhtcSgI2~J_(@aV=FGKyGeVhC_4by z8P;*E8yx^=?MYS4CaJh9ufP&ZsM#!ox*fO)BH7QCE1gyLc&lPSn+hTbH6T z{*1b=XoasK@NrkIr)lrxzCQ-wCP?xcg5x8V7d!b_mCBNj{*qOe>XVP{7-{$?+-w0x z{zv(^cQ%DT{QYfN0I}fqy@s#`Qb9@DGvrjhjgOy^O{wHl{-~ztd=0_#CfEQ}R_agl zk!2;#M}n0nAD8Oo{FskV=A6ct@$nd0Df%jc?(2=ts|aiw3c48FMgSC8(cn)Mk+}4G z)itq0&?Utw1(uW&W0M#$Mz86fNr8bOd#gr?F-=*0jM?;70H1YC491$qH4mm1d%^=7 zGI5N;u7!bN2w0~|*R>!?dcO$SsNmziFYjJc&MAJQ>*ZZvePA`lXxMp64>oAAsKc}0 z`36LV@2&*IfrXbK{N2}Ms1#Y}&E1gA-JcDqHhu%*-g)LM^XtRs=MQ=2jJHT-%Qav$ z+zZ3@3I@oWfOwCLM&P+-tXlpqKn1YpU4V(Y##SvoH(`N~ zW^oBCSZ=}!mI%0hk^n9V+=!TKd>Dfv=SGyeZ0q`#Jpw^n3O&SJujv)2@yZD%eDQ;q z-WwenE^L8yjCE2HF+0CqV6kS(Y9&*QY@rm{HB~6p+qLm6{=%mnmP)~JImX58A%-bK z#{tSPe#`|7f?(>ov6Jlp@!kvc&uyxUq8iCo7F=wzb}59XhQK?5Bqy@X!|0u}g*pwe z;RO-D!%a@rg%d<0Hf4H3@CVArP%2GK_qm z-nq7q4Bsdd*9xQNS?f8h@uGN%7-Qlp47m4(n#jlJ0^mDef~FKH(T|G@!}memuMQza z=Dx}mX_Y-nMam@1Q#ZO`LA2m_w7nYFI=hz|ul9g)t>{OY+PhmS`^Ty5!~3aM5XA5w z)@6h)+pzHBeAj~4%|kwDjIV-^v4AHWV)3U1CCd=E%QwUC(D+YsE3a4%2#mrwu}cNU0bX9b+1Rg_LZ zds!ke2E2h!Ce^Cib~6z5C6dPl(~r=VzxlT+J#sAMW(M* ziRwOpgsu*###p-pMT>DF9Q7^9rj=C^Kru%25(^8M+h+bws)w(B|49d^_BcT+4SeS2 zqdQ*cqLtygLHC!uhFz>u#0=jz5@!>NqZPpHZ@uWDnp6P(i}jhwml628RQ`kl*o5%| z6+k-3`?(6>`TYvO?lPG|0k9`O#Z(sxV1Z_2uUTlW0P^K(U|w4<*E>8SRyo>5ELXfF z-oIRLoK&P|j@D1|AmD+Pt7(ZAv0oSFy2Wx0+jiKq|HOX1>EZSd+OMfS@$r*lqzygr z(iEb@q$%FhCPo_X5jQj49412MZ45D_CIs5MKc= z*Pr;;cP-a5SO9GCWc#VlbCKl&K712O2D~e!#W*$7(m_*HDzvr4d{4~&R+#qb)JvMB zHH>5Ji036A7W#3|*AN(8;_Jm!29oe1G^vt0lEH3gvQvizlX6MkuYr=9LuJdPJjEt% zz1pk-sGz#kX2c-ALYsvzPHjeMvv{Z8sm&S~2V(~bYqP3^%cQ4V2y|uKCxt#OY_kvX zkc#SuSO>MwHOvRrJ^59hr|xNBG`}7bg+7_UXIJPmE`%qX;w>2W3#J-i{E%QO)IFK% z*+ci_$7lH2kyHI2bq~5l_|!cOjOLMHoOaW*8y2a{kZT|p80s&KuUSV+n0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl - garmin_agl.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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()61
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl()61
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl().261
+
+
+ + + +
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..6c16ac153b --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl - garmin_agl.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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()61
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl()61
mrs_uav_state_estimators::garmin_agl::GarminAgl::~GarminAgl().261
+
+
+ + + +
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..47b62a2612 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.html @@ -0,0 +1,159 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl - garmin_agl.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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          61 :   GarminAgl() : AglEstimator(garmin_agl::name, garmin_agl::frame_id, garmin_agl::package_name), is_core_plugin_(is_core_plugin) {
+      57          61 :   }
+      58             : 
+      59         122 :   ~GarminAgl(void) {
+      60         122 :   }
+      61             : 
+      62             :   void initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) override;
+      63             :   bool start(void) override;
+      64             :   bool pause(void) override;
+      65             :   bool reset(void) override;
+      66             : 
+      67             :   mrs_msgs::Float64Stamped getUavAglHeight() const override;
+      68             :   std::vector<double>      getHeightCovariance() const override;
+      69             : };
+      70             : 
+      71             : }  // namespace garmin_agl
+      72             : 
+      73             : }  // namespace mrs_uav_state_estimators
+      74             : 
+      75             : #endif  // ESTIMATORS_STATE_GARMIN_AGL_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.overview.html new file mode 100644 index 0000000000..01180ec6c9 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.overview.html @@ -0,0 +1,39 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/garmin_agl.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..1d069e474cef144181fc5cb4a7752617234f2348 GIT binary patch literal 380 zcmeAS@N?(olHy`uVBq!ia0vp^0YL1{!VDxu^<<9$DTx4|5ZC|z|E~gq{) z-+wzRyyepwr_aj9N0zi)yIA@DruK$xCbdH!RA#*~FEu*Kt+Mr|wp)bOzeHP(-D@^_ zujRkIGNJuOnSSTr%L_6h^0%b~7kbxKyUa3OW%x$$>|Ep5oPyiLY^#!;A~i6^vZ+X|Ua&y>FQExYwM!Ofs>mTmP>_Uf+f44=fmvnFp3 VG2vV8@g5jd44$rjF6*2Ung9 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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..8dbdee0f44 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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..56a254fb47 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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..a9b38a3f93 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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..86fcfc5f60 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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..a299e7eb1c --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/agl + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/aglHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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..a01657e9c6 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - alt_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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)146
mrs_uav_state_estimators::AltGeneric::~AltGeneric()146
mrs_uav_state_estimators::AltGeneric::~AltGeneric().2146
+
+
+ + + +
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..f3729906bc --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - alt_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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)146
mrs_uav_state_estimators::AltGeneric::~AltGeneric()146
mrs_uav_state_estimators::AltGeneric::~AltGeneric().2146
+
+
+ + + +
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..7ffa20c435 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.html @@ -0,0 +1,245 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - alt_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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         146 :   AltGeneric(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin)
+     115         146 :       : AltitudeEstimator<alt_generic::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) {
+     116         146 :   }
+     117             : 
+     118         292 :   ~AltGeneric(void) {
+     119         292 :   }
+     120             : 
+     121             :   void initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) override;
+     122             :   bool start(void) override;
+     123             :   bool pause(void) override;
+     124             :   bool reset(void) override;
+     125             : 
+     126             :   double getState(const int &state_idx_in) const override;
+     127             :   double getState(const int &state_id_in, const int &axis_in) const override;
+     128             : 
+     129             :   void setState(const double &state_in, const int &state_idx_in) override;
+     130             :   void setState(const double &state_in, const int &state_id_in, const int &axis_in) override;
+     131             : 
+     132             :   states_t getStates(void) const override;
+     133             :   void     setStates(const states_t &states_in) override;
+     134             : 
+     135             :   double getCovariance(const int &state_idx_in) const override;
+     136             :   double getCovariance(const int &state_id_in, const int &axis_in) const override;
+     137             : 
+     138             :   covariance_t getCovarianceMatrix(void) const override;
+     139             :   void         setCovarianceMatrix(const covariance_t &cov_in) override;
+     140             : 
+     141             :   double getInnovation(const int &state_idx) const override;
+     142             :   double getInnovation(const int &state_id_in, const int &axis_in) const override;
+     143             : 
+     144             :   void setDt(const double &dt);
+     145             :   void setInputCoeff(const double &input_coeff);
+     146             : 
+     147             :   void generateRepredictorModels(const double input_coeff);
+     148             : 
+     149             :   void generateA();
+     150             :   void generateB();
+     151             : 
+     152             :   void timeoutOdom(const std::string &topic, const ros::Time &last_msg, const int n_pubs);
+     153             :   void timeoutRange(const std::string &topic, const ros::Time &last_msg, const int n_pubs);
+     154             : 
+     155             :   std::string getNamespacedName() const;
+     156             : 
+     157             :   std::string getPrintName() const;
+     158             : };
+     159             : }  // namespace mrs_uav_state_estimators
+     160             : 
+     161             : #endif  // ESTIMATORS_ALTITUDE_ALT_GENERIC_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.overview.html new file mode 100644 index 0000000000..85adff98de --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.overview.html @@ -0,0 +1,61 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/alt_generic.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..dcf0f4a027a6ab756dec1329ddcee71c4cae2957 GIT binary patch literal 688 zcmV;h0#E&kP)bm(Ppe}^qLnGb+F$2vKD02Gn zh($1dgra5Z8E?E&cRG}^Kx#yMfS#cr8p4)R${TUN=r@|=k$axQF7Ej(BA)90}BGAm^jcLl0&P&><5?5@?l)W15 z*ceT@60t-DKw?Uo)d&Z34^8KOsIJS88X1*Zxdf%Fo*cRt+v{ zz{VnN=>k`O353Z4;lz0bk%xtVdKj)gkW5@kI5QgK1#1)T1NCV5ExZAu!6Gf-yUk(o zI_q696*4h1pK!os(Q5%Z&B8@AX`^Yse#EiF#jklyiXJZuc49O|P?=mpMt2K+B4&}! zDxRsQYO{&!#ej+FIWojMZCbMd#E!FR5=8x3=)}zT{5YwaS~4UJFz(3@Jo&slhq@gc zRlV#zM0B&hYSzt`XtsK>y-#`N|2_WR`Tsn3+qUg*EH8d=+rF8^*);g*{x-0$lK(o|Y&0b=W0s!=7T&CEV>Oy!9ePcyge|x|(9`l-aMLnFyWzjy1 z9kVSV@LD0}Sxbt6RK_&i8dzK_FLFMex4`W9Oz}Zykn(SMU9|PPztmjnaL`kYvE`jB zAM+>D)!$NuGqnlj6xLY@*6Y0R)+;pj!HIF@(PL*ABk5r>>k*&qo>xvX(Os97EA9_V W6RZS^kluj+0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - altitude_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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&)146
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator().2146
+
+
+ + + +
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..7d52409c0a --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - altitude_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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&)146
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator()0
mrs_uav_state_estimators::AltitudeEstimator<3>::~AltitudeEstimator().2146
+
+
+ + + +
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..c8e976b31b --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h.gcov.html @@ -0,0 +1,130 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/altitude_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude - altitude_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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         146 :   AltitudeEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, 1>(altitude::type, name, frame_id) {
+      31         146 :   }
+      32             : 
+      33         146 :   virtual ~AltitudeEstimator(void) {
+      34         146 :   }
+      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..ae9bcdf14a --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-09 22:03:33Functions: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..afe134f16b --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-09 22:03:33Functions: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..2901f2c6e6 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-09 22:03:33Functions: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..611f580dff --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-09 22:03:33Functions: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..3b51f5fa57 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-09 22:03:33Functions: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..e20932c204 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-09 22:03:33Functions: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..1c349ca1b8 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func-sort-c.html @@ -0,0 +1,364 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - correction.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:36972550.9 %
Date:2024-04-09 22:03:33Functions:477166.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Correction<1>::callbackImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getVecInFrame(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >)0
mrs_uav_state_estimators::Correction<1>::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromQuat(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getAvgRtkInitZ(double)0
mrs_uav_state_estimators::Correction<2>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)0
mrs_uav_state_estimators::Correction<2>::resetProcessors()0
mrs_uav_state_estimators::Correction<2>::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromQuat(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getAvgRtkInitZ(double)22
mrs_uav_state_estimators::Correction<2>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)85
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)>)89
mrs_uav_state_estimators::Correction<2>::getName[abi:cxx11]() const178
mrs_uav_state_estimators::Correction<2>::getPrintName[abi:cxx11]() const184
mrs_uav_state_estimators::Correction<1>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)203
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)>)215
mrs_uav_state_estimators::Correction<2>::getNamespacedName[abi:cxx11]() const390
mrs_uav_state_estimators::Correction<1>::getName[abi:cxx11]() const430
mrs_uav_state_estimators::Correction<1>::getPrintName[abi:cxx11]() const491
mrs_uav_state_estimators::Correction<1>::getNamespacedName[abi:cxx11]() const858
mrs_uav_state_estimators::Correction<1>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)2507
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)2523
mrs_uav_state_estimators::Correction<1>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const2523
mrs_uav_state_estimators::Correction<2>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)5300
mrs_uav_state_estimators::Correction<2>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const5344
mrs_uav_state_estimators::Correction<2>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)5361
mrs_uav_state_estimators::Correction<2>::getRawCorrection()6522
mrs_uav_state_estimators::Correction<2>::getProcessedCorrection()6523
mrs_uav_state_estimators::Correction<1>::getRawCorrection()7858
mrs_uav_state_estimators::Correction<1>::getProcessedCorrection()7869
mrs_uav_state_estimators::Correction<2>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)10602
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> >)10627
mrs_uav_state_estimators::Correction<2>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)10628
mrs_uav_state_estimators::Correction<2>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)166838
mrs_uav_state_estimators::Correction<2>::getCorrectionFromPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)170698
mrs_uav_state_estimators::Correction<1>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)174680
mrs_uav_state_estimators::Correction<1>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)174771
mrs_uav_state_estimators::Correction<1>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)174793
mrs_uav_state_estimators::Correction<2>::setR(double)179045
mrs_uav_state_estimators::Correction<2>::getStateId() const179361
mrs_uav_state_estimators::Correction<2>::isHealthy()180919
mrs_uav_state_estimators::Correction<2>::isMsgComing()180921
mrs_uav_state_estimators::Correction<2>::applyCorrection(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, ros::Time const&)182745
mrs_uav_state_estimators::Correction<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)186677
mrs_uav_state_estimators::Correction<1>::callbackRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)242393
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRange(boost::shared_ptr<sensor_msgs::Range_<std::allocator<void> > const>)245892
mrs_uav_state_estimators::Correction<2>::publishCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)365674
mrs_uav_state_estimators::Correction<1>::setR(double)416014
mrs_uav_state_estimators::Correction<1>::getStateId() const416045
mrs_uav_state_estimators::Correction<1>::applyCorrection(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&)419243
mrs_uav_state_estimators::Correction<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)422672
mrs_uav_state_estimators::Correction<1>::isHealthy()440444
mrs_uav_state_estimators::Correction<1>::isMsgComing()440497
mrs_uav_state_estimators::Correction<1>::publishCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)838584
mrs_uav_state_estimators::Correction<2>::getR()910054
mrs_uav_state_estimators::Correction<1>::getR()1254399
+
+
+ + + +
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..a635ec7990 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.func.html @@ -0,0 +1,364 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - correction.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:36972550.9 %
Date:2024-04-09 22:03:33Functions:477166.2 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
mrs_uav_state_estimators::Correction<1>::callbackImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)2507
mrs_uav_state_estimators::Correction<1>::isMsgComing()440497
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>)242393
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>)174680
mrs_uav_state_estimators::Correction<1>::getAvgRtkInitZ(double)22
mrs_uav_state_estimators::Correction<1>::applyCorrection(Eigen::Matrix<double, 1, 1, 0, 1, 1> const&, ros::Time const&)419243
mrs_uav_state_estimators::Correction<1>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)174771
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()7858
mrs_uav_state_estimators::Correction<1>::publishCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)838584
mrs_uav_state_estimators::Correction<1>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)2523
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>)245892
mrs_uav_state_estimators::Correction<1>::getProcessedCorrection()7869
mrs_uav_state_estimators::Correction<1>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)203
mrs_uav_state_estimators::Correction<1>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)174793
mrs_uav_state_estimators::Correction<1>::getCorrectionFromOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getCorrectionFromPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<1>::getR()1254399
mrs_uav_state_estimators::Correction<1>::setR(double)416014
mrs_uav_state_estimators::Correction<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)422672
mrs_uav_state_estimators::Correction<1>::isHealthy()440444
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)>)215
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>)5300
mrs_uav_state_estimators::Correction<2>::isMsgComing()180921
mrs_uav_state_estimators::Correction<2>::callbackPoint(boost::shared_ptr<geometry_msgs::PointStamped_<std::allocator<void> > const>)166838
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> >)10627
mrs_uav_state_estimators::Correction<2>::callbackVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)10602
mrs_uav_state_estimators::Correction<2>::getAvgRtkInitZ(double)0
mrs_uav_state_estimators::Correction<2>::applyCorrection(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, ros::Time const&)182745
mrs_uav_state_estimators::Correction<2>::getZVelUntilted(geometry_msgs::Vector3_<std::allocator<void> > const&, std_msgs::Header_<std::allocator<void> > const&)0
mrs_uav_state_estimators::Correction<2>::resetProcessors()0
mrs_uav_state_estimators::Correction<2>::callbackOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getRawCorrection()6522
mrs_uav_state_estimators::Correction<2>::publishCorrection(mrs_uav_state_estimators::Correction<2>::MeasurementStamped const&, mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection_<std::allocator<void> > >&)365674
mrs_uav_state_estimators::Correction<2>::callbackPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::callbackToggleRange(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromImu(boost::shared_ptr<sensor_msgs::Imu_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromRtk(boost::shared_ptr<mrs_msgs::RtkGps_<std::allocator<void> > const>)5361
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>)170698
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()6523
mrs_uav_state_estimators::Correction<2>::createProcessorFromName(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::NodeHandle&)85
mrs_uav_state_estimators::Correction<2>::getCorrectionFromVector(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)10628
mrs_uav_state_estimators::Correction<2>::getCorrectionFromOdometry(boost::shared_ptr<nav_msgs::Odometry_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getCorrectionFromPoseStamped(boost::shared_ptr<geometry_msgs::PoseStamped_<std::allocator<void> > const>)0
mrs_uav_state_estimators::Correction<2>::getR()910054
mrs_uav_state_estimators::Correction<2>::setR(double)179045
mrs_uav_state_estimators::Correction<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)186677
mrs_uav_state_estimators::Correction<2>::isHealthy()180919
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)>)89
mrs_uav_state_estimators::Correction<1>::getStateId() const416045
mrs_uav_state_estimators::Correction<1>::getPrintName[abi:cxx11]() const491
mrs_uav_state_estimators::Correction<1>::getNamespacedName[abi:cxx11]() const858
mrs_uav_state_estimators::Correction<1>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const2523
mrs_uav_state_estimators::Correction<1>::getName[abi:cxx11]() const430
mrs_uav_state_estimators::Correction<2>::getStateId() const179361
mrs_uav_state_estimators::Correction<2>::getPrintName[abi:cxx11]() const184
mrs_uav_state_estimators::Correction<2>::getNamespacedName[abi:cxx11]() const390
mrs_uav_state_estimators::Correction<2>::transformRtkToFcu(geometry_msgs::PoseStamped_<std::allocator<void> > const&) const5344
mrs_uav_state_estimators::Correction<2>::getName[abi:cxx11]() const178
+
+
+ + + +
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..3fd4669463 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/correction.h.gcov.html @@ -0,0 +1,1944 @@ + + + + + + + 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:36972550.9 %
Date:2024-04-09 22:03:33Functions:477166.2 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

TQUV$<8al27)Tw-p1U-Hu9xiGK$@c2FLn~@>=lz&e+n8I@lBMxon`xwZelflL z1?^po5!na;hXQO!okqD|9*?JQ)B1P)F|V(lvwLT40PrEU%VU-SBH+ynBj={Mz=s^K z2ONgwZ}!rJ@t(|z!wUfuOT#b@-aw6u{KsXiS&SHM%~a@B8wxZ$leYhvah=5CIN}7( zjA1^Cm)pX>g_&F*CsG@C(Z>o)F*wnsIYvt8LR-Bgts!a;Bj5t#J2aA10y)Aq$5$_z z&rXgcV1!)8X5LNU#5^HHCQYsZYGpD2n!s)2f)^yPJpy?hX%+&V*wFPYdO>L>QVNOn zaVS=ZG4#HJJ|^)*>3}>qSg&tM1|@}ow>vhZ;y985rvZ%(Faz9J36EtIt(Ru-{c5j*r z%Lh&5x*$E&#mHl{8OV%Aj>1HvNaUZ;_APUPp-L7 z=KaT-9c(Wiay;%~V3+@QkYlwz@VD5Q^q`UDtb@5a~KlQF#p|?dy*YIdRlh%KZjnDtB&&1O*XBCcQgFZ38cp zc>HhkL)6I~_FR>6BwYJgYl!8j+6uT$;nvGwDF7}J(7K;7)96M8m?=doqz`Kx3 z2r^H)%nRJLVj0PUE{i#tq0&yCn`476Xq?#rt&bLt{8q*R8kOgEk?2Bz_Mr}gssq+! zQ>cJlrt4uVr7~m}k0C@^*>Iuzp5n=6ly;gvbjmyMeKmmS$HB;?jIkywWfRXM*&YF3 zvOVh9)Yu+ldpNe^H1_En95sQCIr5op;X3zP=o$wM=JY zX8gz$u}4ZN)+Jh1iU%`>6yR))H8)n=NAtkBxz%JUY+XN)0pL-7rUAgjoCe@i3_$K4 zo(4b$G`HRADe_v1a`Ycy^C2NCh0yGtV@1SP4=2ax-R#X0DYMB$KV8n~-6skjJ3h<-6J<6vbJcVLO$$ea4dojwY zX;U|^eZbk@1J;ulO&?_d9+VSD4C)6saS>ryG2Nf=maA$yRB?}S3u~$sZPx6D){b{ZOalY_tT!l|IDL2ZeppTp!?NdaAcbCodf5KZn%zdQd9^<9#yhA!S72mk2 zxP8H1!7oS+J%HQC%tEnz-q!UE>!XbE<7Q!g{`#DSjA$U&=Z>8QG`GFeF8@{D_1KO0 z-nkEYWX{|N4`^;%ju*vP!JH!Nb9QX7ZA#3S{6FpR`4EV!DR zSzL4Xf*culdL*7CS^p-$Nv>`DijfIRt0r;sDgvq+X-f?f#!vu$NClgi2VXxus)9`# z4-DvEA_t8Ew0LItxElz_m1T+aETryqxseCI%QFNXP}iS}(JM?C9jSb!c!d*ZtYEzu zd#A-P!M&a6!+t+KA*H_7VLCv6+Ujl$UC@?aE#Spop-4EwAHT#W^~`9ya#Dw zu-ch1tIqDGnfIM5)Qpe}p=&j27E33zkuhRS6*0;s(0MhERN`-B=3!%}3ZrTF>M)`~ z&;iAyVLjkPd`JRH9It>&YdaTQ&t^I5>3BbcU^X!OJq);Zn=JCY?rNO>908L%Ow~9} z5l}U0LCsXPK!Du7H3O7K`w|&YtS+W8%JLabsn7muz!`OIz^L12rx_Lp1d=GtEYm-b`{gdNOA2s6n)Cqco$0? zWB2~^CGK3gC~?Xw7X>Jcjy>EgMG{qN-4HSTWjVK&Pp__q%qi( zIo^>6YF#Zh$-yhCm{{Hw227)V#Yq*7mgnF!tt1T-;arB_gEKee)UQ8W)_n3&l#oWh7%ysgT#Qlf z$KKIvNq1W$NuH&eS-Ae*BuPoknIs8@nSWGe>m*5*o%7$xRmslGnN86!{!ysueZbF; zNu3M(`|XV5wwzq&oMAEk;G8)ZBo^?DNlD^4V^aS`Oe*kvTHN>G%E(dtQeZ1HyN4hCpnN`@2HB)$;`rde5PLONc+|*IH^pYU z$UW$gToS^em&x0+!sWfCH zet!N}2OHAGci&z|`g{}cEI4(#@ynqmd;8&p*zSfjpm=tm!LeN0CqK!>2;q=aQQD3i)#?I11{nv$n8Pg%MH7ruT0SvYgh$ zQzhyEwTx;6RGC@Yxxbd$@%w8fP+80wMj?N3&gd7_dm8V!F^?L#S!w|spkecanTg(k z0C>*p-=Eht;jmJQvzK9biqdmBqg)CzpAh9bD|{(Hb2}@16^sW~_-6aw@NqT>c4p4q zhla7*lM=S!jLX#q@N{Qbj6dVf{B%9VJ_n3N!1I7Hi}7dXjI*M)hh$}ot}k>S@j6wr z1vu{cGv*c@nQ(QLdyYcqpJLiy-5V=lXBooJ@Xh3VuNrItup{>STL8*$*kuc_nj0?HIfG#!Uskf&HsZMY*f6Gjnb#G>lc7ilXEMj_UF-uoRwECJqYEO8XQD$US{B z15}fCOEb*v9U{YF;+T~8bmFA;@f)9C%+Hk3xI3O~KWYG?4bK+$5j+b1XDmhF*(RP$ z8K{lQe{U&%@a&!vA~9SO<$h#mGznt4!x!N02X|pY!3@IuBr&!+r2#wKeB2GT9S&c3 zgxfE50?E1cAEeRH^?5)~W6W zx)|n{{i}~}H?;MVdJ7)@BK6Dn|l)~8ggWa(fJ)3RQ7zVNuTOEwSFTTpj4%X(fZ(a zYs-pKq^|wvB84E4ML#lR8|qfobft>SJ^)NFCiud0xJAuOp(YE)Vv~#R ze%OG~bg4?6z|A!7qL0L7^Cd=eh{W7l`+EC$d-?K8uTM2~0A_BwFopZ86!*C06meaH zT$oJbUVyhzTRf4g0Nl~y;@!(Fhhi}r8-L?&R$5YDF(lSgLqOgGhk*Otk2`o;$MyV~ zJAxK#kbd&o+lB{ipcHu}+QU*PsJQ5(eyh0LuPGFetM z5Mwfn%FG4*ph~tbNUirIT#f0-y^d6LhBX3ClwcDvBZwKj>z$bCEi;*Bro*=WLzn?d zME6ptSy;0=m-p^MrtCm9vx>1p7K)#ul5N;Z;vZe5U&IfpcEceNMTD%p<9#InG14Br5rpwSB@&1{BYal0{l{%1_b>Q_O9EYSl zp^<<)PA+YVRZK2wYL#zQYIwR|l-F>OLn2$sX}SbZJiW=GR4}FiD=>0MRT{w^8W~3Z zSha{TezGM-8qLE53`CrD0I=2MN+)ut21<*LKLTYqW%K5ORJ6+C5ML&30aS`?>lUOj ztcFS^7p8e`;V`uiI;{@|aQb_R2T3}`NQFcG>7ov*jegF^p~pbu{hsEa6rQ!_hHr|^ zfn5)}6miHdME5m7Zy3hq8v|Z>#u)n4*RG4{dV#EKRVmC}>;2xU&MSDKs2D#ZrO~Kl zkhQm2HF8Z+Y^V#>i(;qLTR49-hnmv((1?rK0F;%|T-VkG$-K@ybsy6KZW-%lr5px7 z;Vv2b?E7|R$|nQSX5-*H*w8wmREOQmJSqzq7bDJzOWzi&Qvt@CV(b$|CO|YaVXW{W zmNgGF3}d;lqE&!8?LrEu02~+8L@nK}=a7^I=_8E2a{AM30NAXQH?EZ#sTF08kxw=m z1+?E0Q7~?yuHCuli&_a4Vj~b=C&X}n#REW16S1s^UOrD Q>;M1&07*qoM6N<$f@mKcN&o-= 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..fb22c6a2e2 --- /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-04-09 22:03:33Functions: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..13d246670f --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:050.0 %
Date:2024-04-09 22:03:33Functions: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..37b5eefe34 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.html @@ -0,0 +1,243 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:050.0 %
Date:2024-04-09 22:03:33Functions:030.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_generic.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..c826730ad70614286d57a68cb29d1eed954df391 GIT binary patch literal 663 zcmV;I0%-k-P)4zK6(s~1FktrePxJa#)*WXO@}bs+5PYc2AyWg@7ASK1 zmo$>`V`ShNVzi=_j5E@Ga0&E!#xAr5cGX+pFeHTm>DC^BgH2eKUSeGGUW|YR z7&{Nn{v{A*73@1M0FWYBF}}EpfuKx5)^W|@6!sV!tj=T%G@_9|!6&8Y7|lqhO`wNH z;HZh3g-;(@)7!jR#8}J%xfo%I*N{%(K31_-x+M}G^;wH+^4D%7$DuvpqLY;~ z-0$njF*!iuxSFPdXk6j0=qfLdi_%n+A*+D#!T7|JPxr`>pUGyW&0YqINimidlfp*2 zo;|&|SkS7s)fC%t+fVx3t7C + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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)85
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough()85
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough().285
+
+
+ + + +
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..36cce8f35f --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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)85
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough()85
mrs_uav_state_estimators::HdgPassthrough::~HdgPassthrough().285
+
+
+ + + +
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..b5c528870c --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h.gcov.html @@ -0,0 +1,191 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/hdg_passthrough.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - hdg_passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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          85 :   HdgPassthrough(const std::string &name, const std::string &ns_frame_id, const std::string &parent_state_est_name, const bool is_core_plugin)
+      70          85 :       : HeadingEstimator<hdg_passthrough::n_states>(name, ns_frame_id), parent_state_est_name_(parent_state_est_name), is_core_plugin_(is_core_plugin) {
+      71          85 :   }
+      72             : 
+      73         170 :   ~HdgPassthrough(void) {
+      74         170 :   }
+      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..0d3a741fe4 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func-sort-c.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - heading_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22100.0 %
Date:2024-04-09 22:03:33Functions: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&)85
+
+
+ + + +
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..07937ae4df --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.func.html @@ -0,0 +1,84 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - heading_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22100.0 %
Date:2024-04-09 22:03:33Functions: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&)85
+
+
+ + + +
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..a0bb5f690f --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h.gcov.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/heading_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading - heading_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:22100.0 %
Date:2024-04-09 22:03:33Functions: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          85 :   HeadingEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, 1>(heading::type, name, frame_id) {
+      23          85 :   }
+      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..d7fb5c6023 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-f.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-04-09 22:03:33Functions: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..c9187b6dff --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail-sort-l.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-04-09 22:03:33Functions: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..5d55aa1a31 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-detail.html @@ -0,0 +1,138 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-04-09 22:03:33Functions: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..28b298835a --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-f.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-04-09 22:03:33Functions: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..db5d87f85a --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index-sort-l.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-04-09 22:03:33Functions: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..c3fdc8b566 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading/index.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/heading + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/headingHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71258.3 %
Date:2024-04-09 22:03:33Functions: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..de168fe992 --- /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:40376153.0 %
Date:2024-04-09 22:03:33Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
50.9%50.9%
+ 		50.9 %369 / 72566.2 %47 / 71
<unnamed>50.9 %369 / 72566.2 %47 / 71
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
<unnamed>94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail-sort-l.html new file mode 100644 index 0000000000..2fe095ca8b --- /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:40376153.0 %
Date:2024-04-09 22:03:33Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
50.9%50.9%
+ 		50.9 %369 / 72566.2 %47 / 71
<unnamed>50.9 %369 / 72566.2 %47 / 71
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
<unnamed>94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-detail.html new file mode 100644 index 0000000000..bb1447d796 --- /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:40376153.0 %
Date:2024-04-09 22:03:33Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
50.9%50.9%
+ 		50.9 %369 / 72566.2 %47 / 71
<unnamed>50.9 %369 / 72566.2 %47 / 71
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
<unnamed>94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-f.html new file mode 100644 index 0000000000..ef00843280 --- /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:40376153.0 %
Date:2024-04-09 22:03:33Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
50.9%50.9%
+ 		50.9 %369 / 72566.2 %47 / 71
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-l.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index-sort-l.html new file mode 100644 index 0000000000..3c78f3081b --- /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:40376153.0 %
Date:2024-04-09 22:03:33Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
50.9%50.9%
+ 		50.9 %369 / 72566.2 %47 / 71
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/index.html new file mode 100644 index 0000000000..f289dafa36 --- /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:40376153.0 %
Date:2024-04-09 22:03:33Functions:669569.5 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
correction.h +
50.9%50.9%
+ 		50.9 %369 / 72566.2 %47 / 71
partial_estimator.h +
94.4%94.4%
+ 		94.4 %34 / 3679.2 %19 / 24
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-f.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-f.html new file mode 100644 index 0000000000..7e851d3222 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1010100.0 %
Date:2024-04-09 22:03:33Functions: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..668296a2e3 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1010100.0 %
Date:2024-04-09 22:03:33Functions: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..11fcb35e41 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1010100.0 %
Date:2024-04-09 22:03:33Functions: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..3de83c469d --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1010100.0 %
Date:2024-04-09 22:03:33Functions: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..4b80df2b80 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1010100.0 %
Date:2024-04-09 22:03:33Functions: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..05285c1516 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateralHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:1010100.0 %
Date:2024-04-09 22:03:33Functions: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..61268b75aa --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lat_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-04-09 22:03:33Functions: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> ()>)85
mrs_uav_state_estimators::LatGeneric::~LatGeneric()85
mrs_uav_state_estimators::LatGeneric::~LatGeneric().285
+
+
+ + + +
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..794411e93d --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lat_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-04-09 22:03:33Functions: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> ()>)85
mrs_uav_state_estimators::LatGeneric::~LatGeneric()85
mrs_uav_state_estimators::LatGeneric::~LatGeneric().285
+
+
+ + + +
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..22d512b50c --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.html @@ -0,0 +1,250 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lat_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:66100.0 %
Date:2024-04-09 22:03:33Functions: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          85 :   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          85 :       : 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          85 :         fun_get_hdg_(fun_get_hdg) {
+     124          85 :   }
+     125             : 
+     126         170 :   ~LatGeneric(void) {
+     127         170 :   }
+     128             : 
+     129             :   void initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) override;
+     130             :   bool start(void) override;
+     131             :   bool pause(void) override;
+     132             :   bool reset(void) override;
+     133             : 
+     134             :   double getState(const int &state_idx_in) const override;
+     135             :   double getState(const int &state_id_in, const int &axis_in) const override;
+     136             : 
+     137             :   void setState(const double &state_in, const int &state_idx_in) override;
+     138             :   void setState(const double &state_in, const int &state_id_in, const int &axis_in) override;
+     139             : 
+     140             :   states_t getStates(void) const override;
+     141             :   void     setStates(const states_t &states_in) override;
+     142             : 
+     143             :   double getCovariance(const int &state_idx_in) const override;
+     144             :   double getCovariance(const int &state_id_in, const int &axis_in) const override;
+     145             : 
+     146             :   covariance_t getCovarianceMatrix(void) const override;
+     147             :   void         setCovarianceMatrix(const covariance_t &cov_in) override;
+     148             : 
+     149             :   double getInnovation(const int &state_idx) const override;
+     150             :   double getInnovation(const int &state_id_in, const int &axis_in) const override;
+     151             : 
+     152             :   void setDt(const double &dt);
+     153             :   void setInputCoeff(const double &input_coeff);
+     154             : 
+     155             :   void generateRepredictorModels(const double input_coeff);
+     156             : 
+     157             :   void generateA();
+     158             :   void generateB();
+     159             : 
+     160             :   std::string getNamespacedName() const;
+     161             : 
+     162             :   std::string getPrintName() const;
+     163             : };
+     164             : }  // namespace mrs_uav_state_estimators
+     165             : 
+     166             : #endif  // ESTIMATORS_LATERAL_LAT_GENERIC_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.overview.html new file mode 100644 index 0000000000..f10b1f5fd4 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.overview.html @@ -0,0 +1,62 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lat_generic.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..6fa70431bbed0c82bdfc8693d0379281e7228df2 GIT binary patch literal 708 zcmV;#0z3VQP)00{{R3zw{NF0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpKi;y_O zOB%{}8Z7WkVq8Vb85a;TMu+qO%z?qqs6t!d`_Ki(-X4L2*k+}t7^nPr@-bN4TCPl_ zAGp6E9d7CK7+_eqUf?z7BcRy=1apRLR>%N^9({@URod!l4E(Mq!gNnhOh#%nr`xJF z0&gcCa`_Kj2fR~XXGX)?Of0kTg4lj1u0XNaX4Yj}%I_Dq9E>oi0UM9B2aPcLmq6+; z8SRj4#@J{W09l#!j6(GZlPQ;|JTn?&hqW1v0Y)_RlK8rk90QMZNdi37H%47_7c7NB zOri%l;k@Xz1ZB76BAWEk^e%km5%UsOeFKDr$-Ws)8B`WmUuTojJrVIpXJz-)Q?=Qo z_U~}{q)@~Zrvmbe*OZrB)>9@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lateral_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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&)85
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator().285
+
+
+ + + +
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..526ce2557c --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lateral_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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&)85
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator()0
mrs_uav_state_estimators::LateralEstimator<6>::~LateralEstimator().285
+
+
+ + + +
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..48532929ef --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h.gcov.html @@ -0,0 +1,122 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral/lateral_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/lateral - lateral_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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          85 :   LateralEstimator(const std::string& name, const std::string& frame_id) : PartialEstimator<n_states, lateral::n_axes>(lateral::type, name, frame_id) {
+      24          85 :   }
+      25             : 
+      26          85 :   ~LateralEstimator(void) {
+      27          85 :   }
+      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..40cd8f9032 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func-sort-c.html @@ -0,0 +1,176 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - partial_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:343694.4 %
Date:2024-04-09 22:03:33Functions: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&)85
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator().285
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&)85
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator().285
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&)146
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator().2146
mrs_uav_state_estimators::PartialEstimator<3, 1>::stateIdToIndex(int const&, int const&) const133088
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&) const155424
mrs_uav_state_estimators::PartialEstimator<6, 2>::getCovarianceAsVector() const155424
mrs_uav_state_estimators::PartialEstimator<6, 2>::publishOutput() const155437
mrs_uav_state_estimators::PartialEstimator<6, 2>::getStatesAsVector() const155439
mrs_uav_state_estimators::PartialEstimator<2, 1>::getCovarianceAsVector() const173558
mrs_uav_state_estimators::PartialEstimator<2, 1>::getStatesAsVector() const173567
mrs_uav_state_estimators::PartialEstimator<2, 1>::publishOutput() const173569
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&) const267046
mrs_uav_state_estimators::PartialEstimator<3, 1>::getCovarianceAsVector() const267058
mrs_uav_state_estimators::PartialEstimator<3, 1>::publishOutput() const267151
mrs_uav_state_estimators::PartialEstimator<3, 1>::getStatesAsVector() const267244
mrs_uav_state_estimators::PartialEstimator<6, 2>::stateIdToIndex(int const&, int const&) const2349930
+
+
+ + + +
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..da0db527d9 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.func.html @@ -0,0 +1,176 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - partial_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:343694.4 %
Date:2024-04-09 22:03:33Functions: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&)85
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<2, 1>::~PartialEstimator().285
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&)146
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<3, 1>::~PartialEstimator().2146
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&)85
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator()0
mrs_uav_state_estimators::PartialEstimator<6, 2>::~PartialEstimator().285
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() const173569
mrs_uav_state_estimators::PartialEstimator<2, 1>::stateIdToIndex(int const&, int const&) const0
mrs_uav_state_estimators::PartialEstimator<2, 1>::getStatesAsVector() const173567
mrs_uav_state_estimators::PartialEstimator<2, 1>::getCovarianceAsVector() const173558
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&) const267046
mrs_uav_state_estimators::PartialEstimator<3, 1>::publishOutput() const267151
mrs_uav_state_estimators::PartialEstimator<3, 1>::stateIdToIndex(int const&, int const&) const133088
mrs_uav_state_estimators::PartialEstimator<3, 1>::getStatesAsVector() const267244
mrs_uav_state_estimators::PartialEstimator<3, 1>::getCovarianceAsVector() const267058
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&) const155424
mrs_uav_state_estimators::PartialEstimator<6, 2>::publishOutput() const155437
mrs_uav_state_estimators::PartialEstimator<6, 2>::stateIdToIndex(int const&, int const&) const2349930
mrs_uav_state_estimators::PartialEstimator<6, 2>::getStatesAsVector() const155439
mrs_uav_state_estimators::PartialEstimator<6, 2>::getCovarianceAsVector() const155424
+
+
+ + + +
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..9dc5f57b3f --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.html @@ -0,0 +1,264 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators - partial_estimator.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:343694.4 %
Date:2024-04-09 22:03:33Functions: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         316 :   PartialEstimator(const std::string &type, const std::string &name, const std::string &frame_id) : Estimator(type, name, frame_id) {
+      62         316 :   }
+      63             : 
+      64         316 :   ~PartialEstimator(void) {
+      65         316 :   }
+      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      596250 : std::vector<double> PartialEstimator<n_states, n_axes>::getStatesAsVector(void) const {
+     103      596250 :   const states_t      states = getStates();
+     104      596044 :   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     2676612 :   for (int i = 0; i < states.size(); i++) {
+     109     2079960 :     states_vec.push_back(states(i));
+     110             :   }
+     111     1191500 :   return states_vec;
+     112             : }
+     113             : /*//}*/
+     114             : 
+     115             : /*//{ getCovarianceAsvector() */
+     116             : template <int n_states, int n_axes>
+     117      596040 : std::vector<double> PartialEstimator<n_states, n_axes>::getCovarianceAsVector(void) const {
+     118      596040 :   const covariance_t  covariance = getCovarianceMatrix();
+     119      595903 :   std::vector<double> covariance_vec;
+     120             :   /* for (auto cov : covariance.reshaped<Eigen::RowMajor>(covariance.size())) { */
+     121             :   /*   covariance_vec.push_back(*cov); */
+     122             :   /* } */
+     123     2674962 :   for (int i = 0; i < covariance.rows(); i++) {
+     124    10770009 :     for (int j = 0; j < covariance.cols(); j++) {
+     125     8689164 :       covariance_vec.push_back(covariance(i, j));
+     126             :     }
+     127             :   }
+     128     1191642 :   return covariance_vec;
+     129             : }
+     130             : /*//}*/
+     131             : 
+     132             : /*//{ stateIdToIndex() */
+     133             : template <int n_states, int n_axes>
+     134     2483018 : int PartialEstimator<n_states, n_axes>::stateIdToIndex(const int &state_id_in, const int &axis_in) const {
+     135     2483018 :   return state_id_in * _n_axes_ + axis_in;
+     136             : }
+     137             : /*//}*/
+     138             : 
+     139             : /*//{ publishOutput() */
+     140             : template <int n_states, int n_axes>
+     141      596157 : void PartialEstimator<n_states, n_axes>::publishOutput() const {
+     142             : 
+     143      596157 :   if (!ch_->debug_topics.output) {
+     144           0 :     return;
+     145             :   }
+     146             : 
+     147     1192171 :   mrs_msgs::EstimatorOutput msg;
+     148      595778 :   msg.header.stamp    = ros::Time::now();
+     149      596303 :   msg.header.frame_id = getFrameId();
+     150      596229 :   msg.state           = getStatesAsVector();
+     151      595388 :   msg.covariance      = getCovarianceAsVector();
+     152             : 
+     153      595601 :   ph_output_.publish(msg);
+     154             : }
+     155             : /*//}*/
+     156             : 
+     157             : /*//{ publishInput() */
+     158             : template <int n_states, int n_axes>
+     159             : template <typename u_t>
+     160      422470 : void PartialEstimator<n_states, n_axes>::publishInput(const u_t &u, const ros::Time& stamp) const {
+     161             : 
+     162      422470 :   if (!ch_->debug_topics.input) {
+     163           0 :     return;
+     164             :   }
+     165             : 
+     166      841867 :   mrs_msgs::Float64ArrayStamped msg;
+     167      419225 :   msg.header.stamp = stamp;
+     168      995574 :   for (int i = 0; i < u.rows(); i++) {
+     169      572812 :     msg.values.push_back(u(i));
+     170             :   }
+     171             : 
+     172      419659 :   ph_input_.publish(msg);
+     173             : }
+     174             : /*//}*/
+     175             : 
+     176             : /*//}*/
+     177             : 
+     178             : }  // namespace mrs_uav_state_estimators
+     179             : 
+     180             : #endif  // ESTIMATORS_PARTIAL_ESTIMATOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.overview.html new file mode 100644 index 0000000000..53598245fe --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.overview.html @@ -0,0 +1,65 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/partial_estimator.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..93d0059d99c5276a3d7504dec305b703e9c2544a GIT binary patch literal 820 zcmV-41Izr0P)#@C3`iKnVY{!^>+5Ti^XbD$B2m^_W4WoO!9duo z-Z{-(9>Tbpej>C6<5QG`Q7+>dxK-<6*Z26^G2WBM?U2rlwQ_sF$2}iC0|0T?a9!ck zg>`^>HmCq%_a91pWDKyA&VN=4)q}|BGh<*HIJKkuYKn{|reXw2nyRkHw7C)p9=|=8 zJf37-gC?-NI3Fb>v4-o~l*%hZ1|H`-6=~9h9*J&(k;+U6hE`fcpJ2VZX3tx*J~F4_ z6Fjok@yeitW}v`5P9f_8l%n83IAW0yE8emebLqF=f7bZh?LTVq3uFp|1t4+l1@=F1 ztt?uhL$*njS;MT?!ZhjtP5#@=E}-~KCQPG&Mu7rNnwV3r%M>8lavpo!J_zved3D+H zI$F{d)j;S2ODoMy#2h={=7dCRL$fY(AX4Y&-flD0o}gsJa?UZ`?RXU(XhtvXO0enC9pfUx%Sy + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-09 22:03:33Functions: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..441304b208 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-09 22:03:33Functions: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..1cdb33bee1 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-09 22:03:33Functions: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..0fe7cd3e72 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-09 22:03:33Functions: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..96cdd24274 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-09 22:03:33Functions: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..72d2e84454 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:99100.0 %
Date:2024-04-09 22:03:33Functions: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..bbf1080fb0 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state - passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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..fb5e3c211c --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state - passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions: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..df39a83ed0 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.html @@ -0,0 +1,173 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state - passthrough.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:44100.0 %
Date:2024-04-09 22:03:33Functions:33100.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/passthrough.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..e44c5c845709562b6df9a8255e423f99ae0ac942 GIT binary patch literal 431 zcmV;g0Z{&lP)0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpfO*y3~prpJEFVmUx_krB1ErfufwoNp%&};z`6IVHe zcp7+wCJ?=_l=#9mnq$p<=SVIGyJdG{!>EreX941!LoLd-1=^Nd$^p8)B+kWgO+ibE+Soi~>yX!XLzaeKefFguZm-q|K zRsu&L69)Xr&V>2cwV0%$x?)mMi23O}bC|&P2;J)0x8LW + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state - state_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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)85
mrs_uav_state_estimators::StateGeneric::~StateGeneric().285
+
+
+ + + +
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..64043d2bdb --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state - state_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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)85
mrs_uav_state_estimators::StateGeneric::~StateGeneric()0
mrs_uav_state_estimators::StateGeneric::~StateGeneric().285
+
+
+ + + +
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..91d6e5dcb2 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h.gcov.html @@ -0,0 +1,183 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state/state_generic.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/estimators/state - state_generic.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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          85 :   StateGeneric(const std::string &name, const bool is_core_plugin)
+      79          85 :       : StateEstimator(name, name + "_origin", state_generic::package_name), is_core_plugin_(is_core_plugin) {
+      80          85 :   }
+      81             : 
+      82          85 :   ~StateGeneric(void) {
+      83          85 :   }
+      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..c55eb9efe4 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-f.html @@ -0,0 +1,182 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13318472.3 %
Date:2024-04-09 22:03:33Functions:173450.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
proc_excessive_tilt.h +
70.7%70.7%
+ 		70.7 %29 / 4133.3 %2 / 6
<unnamed>70.7 %29 / 4133.3 %2 / 6
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
<unnamed>63.9 %23 / 3633.3 %2 / 6
proc_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..3f4a3d1839 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail-sort-l.html @@ -0,0 +1,182 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13318472.3 %
Date:2024-04-09 22:03:33Functions:173450.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
<unnamed>63.9 %23 / 3633.3 %2 / 6
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..f657b9ee07 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-detail.html @@ -0,0 +1,182 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13318472.3 %
Date:2024-04-09 22:03:33Functions:173450.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
proc_excessive_tilt.h +
70.7%70.7%
+ 		70.7 %29 / 4133.3 %2 / 6
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.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..6cda52b2b2 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index-sort-l.html @@ -0,0 +1,142 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13318472.3 %
Date:2024-04-09 22:03:33Functions:173450.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
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..c7391011e9 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/index.html @@ -0,0 +1,142 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processorsHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:13318472.3 %
Date:2024-04-09 22:03:33Functions:173450.0 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
proc_excessive_tilt.h +
70.7%70.7%
+ 		70.7 %29 / 4133.3 %2 / 6
proc_median_filter.h +
63.9%63.9%
+ 		63.9 %23 / 3633.3 %2 / 6
proc_saturate.h +
72.5%72.5%
+ 		72.5 %29 / 4066.7 %4 / 6
proc_tf_to_world.h +
78.9%78.9%
+ 		78.9 %45 / 5737.5 %3 / 8
processor.h +
70.0%70.0%
+ 		70.0 %7 / 1075.0 %6 / 8
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func-sort-c.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_excessive_tilt.h.func-sort-c.html new file mode 100644 index 0000000000..7cf5465aaf --- /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-04-09 22:03:33Functions: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&)67
mrs_uav_state_estimators::ProcExcessiveTilt<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)130648
+
+
+ + + +
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..c9428c9e17 --- /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-04-09 22:03:33Functions: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>&)130648
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&)67
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..ecd2761712 --- /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-04-09 22:03:33Functions: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          67 : 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          67 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      45             : 
+      46             :   // | --------------------- load parameters -------------------- |
+      47          67 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      48             : 
+      49          67 :   ph->param_loader->loadParam("orientation_topic", orientation_topic_);
+      50             :   double max_tilt;
+      51          67 :   ph->param_loader->loadParam("max_tilt", max_tilt);
+      52             : 
+      53          67 :   max_tilt = M_PI * (max_tilt / 180.0);
+      54             : 
+      55          67 :   max_tilt_sq_ = std::pow(max_tilt, 2);
+      56             : 
+      57          67 :   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          67 :   mrs_lib::SubscribeHandlerOptions shopts;
+      64          67 :   shopts.nh                 = nh;
+      65          67 :   shopts.node_name          = Processor<n_measurements>::getPrintName();
+      66          67 :   shopts.no_message_timeout = ros::Duration(1.0);
+      67          67 :   shopts.threadsafe         = true;
+      68          67 :   shopts.autostart          = true;
+      69          67 :   shopts.queue_size         = 10;
+      70          67 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      71             : 
+      72          67 :   sh_orientation_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "/" + ch->uav_name + "/" + orientation_topic_);
+      73          67 : }
+      74             : /*//}*/
+      75             : 
+      76             : /*//{ process() */
+      77             : template <int n_measurements>
+      78      130648 : std::tuple<bool, bool> ProcExcessiveTilt<n_measurements>::process([[maybe_unused]] measurement_t& measurement) {
+      79             : 
+      80      130648 :   if (!Processor<n_measurements>::enabled_) {
+      81           0 :     return {true, true};
+      82             :   }
+      83             : 
+      84      130648 :   if (!sh_orientation_.hasMsg()) {
+      85           0 :     return {false, false};
+      86             :   }
+      87             : 
+      88      130643 :   bool ok_flag     = true;
+      89      130643 :   bool should_fuse = true;
+      90             : 
+      91             :   try {
+      92      130643 :     Eigen::Matrix3d orientation_R = mrs_lib::AttitudeConverter(sh_orientation_.getMsg()->quaternion);
+      93             : 
+      94      130629 :     const double tilt = mrs_lib::geometry::angleBetween(Eigen::Vector3d(0, 0, 1), orientation_R.col(2));
+      95             : 
+      96      130617 :     const bool is_excessive_tilt = std::pow(tilt, 2) > max_tilt_sq_;
+      97             : 
+      98      130593 :     if (is_excessive_tilt) {
+      99           1 :       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      130645 :   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_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..541c32ad07 --- /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-04-09 22:03:33Functions: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&)67
mrs_uav_state_estimators::ProcMedianFilter<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)130641
+
+
+ + + +
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..33887a9d80 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.func.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_median_filter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:233663.9 %
Date:2024-04-09 22:03:33Functions: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>&)130641
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&)67
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..34122ab719 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h.gcov.html @@ -0,0 +1,192 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_median_filter.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_median_filter.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:233663.9 %
Date:2024-04-09 22:03:33Functions: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          67 : 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          67 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      41             : 
+      42             :   // | --------------------- load parameters -------------------- |
+      43          67 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      44             : 
+      45          67 :   ph->param_loader->loadParam("buffer_size", buffer_size_);
+      46          67 :   ph->param_loader->loadParam("max_diff", max_diff_);
+      47             : 
+      48          67 :   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          67 :   const double min_valid = std::numeric_limits<double>::lowest();
+      55          67 :   const double max_valid = std::numeric_limits<double>::max();
+      56             : 
+      57             :   // initialize median filter
+      58         134 :   for (int i = 0; i < n_measurements; i++) {
+      59          67 :     vec_mf_.push_back(mrs_lib::MedianFilter(buffer_size_, min_valid, max_valid, max_diff_));
+      60             :   }
+      61          67 : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ process() */
+      65             : template <int n_measurements>
+      66      130641 : std::tuple<bool, bool> ProcMedianFilter<n_measurements>::process(measurement_t& measurement) {
+      67             : 
+      68      130641 :   if (!Processor<n_measurements>::enabled_) {
+      69           0 :     return {true, true};
+      70             :   }
+      71             : 
+      72      130641 :   bool ok_flag     = true;
+      73      130641 :   bool should_fuse = true;
+      74      261244 :   for (int i = 0; i < measurement.rows(); i++) {
+      75      130565 :     vec_mf_[i].add(measurement(i));
+      76      130595 :     if (vec_mf_[i].full()) {
+      77      123945 :       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        6633 :       ROS_WARN_THROTTLE(1.0, "[%s]: median filter not full yet", Processor<n_measurements>::getPrintName().c_str());
+      88        6633 :       ok_flag     = false;
+      89        6633 :       should_fuse = false;
+      90             :     }
+      91             :   }
+      92             : 
+      93      130582 :   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..88c8a5f976 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func-sort-c.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_saturate.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:294072.5 %
Date:2024-04-09 22:03:33Functions: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)>)69
mrs_uav_state_estimators::ProcSaturate<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)5365
mrs_uav_state_estimators::ProcSaturate<1>::process(Eigen::Matrix<double, 1, 1, 0, 1, 1>&)133092
+
+
+ + + +
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..ea05f0d80e --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.func.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_saturate.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:294072.5 %
Date:2024-04-09 22:03:33Functions: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>&)133092
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)>)69
mrs_uav_state_estimators::ProcSaturate<2>::reset()0
mrs_uav_state_estimators::ProcSaturate<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)5365
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..783c0be1ca --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h.gcov.html @@ -0,0 +1,202 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_saturate.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_saturate.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:294072.5 %
Date:2024-04-09 22:03:33Functions: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          73 : 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          73 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph), state_id_(state_id), fun_get_state_(fun_get_state) {
+      46             : 
+      47             :   // | --------------------- load parameters -------------------- |
+      48          73 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      49             : 
+      50          73 :   ph->param_loader->loadParam("start_enabled", this->start_enabled_);
+      51          73 :   this->enabled_ = this->start_enabled_;
+      52          73 :   ph->param_loader->loadParam("keep_enabled", keep_enabled_);
+      53          73 :   ph->param_loader->loadParam("min", saturate_min_);
+      54          73 :   ph->param_loader->loadParam("max", saturate_max_);
+      55          73 :   ph->param_loader->loadParam("limit", innovation_limit_);
+      56             : 
+      57          73 :   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          73 : }
+      62             : /*//}*/
+      63             : 
+      64             : /*//{ process() */
+      65             : template <int n_measurements>
+      66      138457 : std::tuple<bool, bool> ProcSaturate<n_measurements>::process(measurement_t& measurement) {
+      67             : 
+      68             :   // if no saturation is required, processing is successful
+      69      138457 :   if (!this->enabled_) {
+      70           0 :     return {true, true};
+      71             :   }
+      72             : 
+      73      138457 :   bool ok_flag     = true;
+      74      138457 :   bool should_fuse = true;
+      75      278001 :   for (int i = 0; i < measurement.rows(); i++) {
+      76      143805 :     const double state = fun_get_state_(state_id_, i);
+      77      143823 :     ROS_INFO_ONCE("[%s]: first state[%d][%d]: %.2f", Processor<n_measurements>::getNamespacedName().c_str(), state_id_, i, state);
+      78             : 
+      79      143823 :     if (measurement(i) > state + innovation_limit_ || measurement(i) < state - innovation_limit_) {
+      80        4254 :       return {true, true};  // do not even try to saturate, trigger innovation-based switch to other estimator
+      81             :     }
+      82             : 
+      83      139492 :     if (measurement(i) > state + saturate_max_) {
+      84         749 :       const double saturated = state + saturate_max_;
+      85         749 :       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         749 :       measurement(i) = saturated;
+      88         749 :       ok_flag        = false;
+      89         749 :       should_fuse    = true;
+      90      138832 :     } 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      134194 :   if (!this->keep_enabled_ && ok_flag) {
+     102           0 :     this->enabled_ = false;
+     103             :   }
+     104             : 
+     105      134194 :   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..ad2e6d0082 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func-sort-c.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_tf_to_world.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:455778.9 %
Date:2024-04-09 22:03:33Functions: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&)81
mrs_uav_state_estimators::ProcTfToWorld<2>::callbackGnss(boost::shared_ptr<sensor_msgs::NavSatFix_<std::allocator<void> > const>)81475
mrs_uav_state_estimators::ProcTfToWorld<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)170665
+
+
+ + + +
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..75860cc112 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.func.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_tf_to_world.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:455778.9 %
Date:2024-04-09 22:03:33Functions: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>)81475
mrs_uav_state_estimators::ProcTfToWorld<2>::reset()0
mrs_uav_state_estimators::ProcTfToWorld<2>::process(Eigen::Matrix<double, 2, 1, 0, 2, 1>&)170665
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&)81
+
+
+ + + +
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..f5c6651c34 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.html @@ -0,0 +1,241 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - proc_tf_to_world.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:455778.9 %
Date:2024-04-09 22:03:33Functions: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          81 : 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          81 :     : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+      51             : 
+      52          81 :   ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+      53             : 
+      54          81 :   ph->param_loader->loadParam("gnss_topic", gnss_topic_);
+      55             : 
+      56          81 :   gnss_topic_ = "/" + ch->uav_name + "/" + gnss_topic_;
+      57             : 
+      58          81 :   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          81 :   mrs_lib::SubscribeHandlerOptions shopts;
+      65          81 :   shopts.nh                 = nh;
+      66          81 :   shopts.node_name          = Processor<n_measurements>::getPrintName();
+      67          81 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      68          81 :   shopts.threadsafe         = true;
+      69          81 :   shopts.autostart          = true;
+      70          81 :   shopts.queue_size         = 10;
+      71          81 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      72             : 
+      73          81 :   sh_gnss_ = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, gnss_topic_, &ProcTfToWorld::callbackGnss, this);
+      74             : 
+      75          81 :   is_initialized_ = true;
+      76          81 : }
+      77             : /*//}*/
+      78             : 
+      79             : /*//{ callbackGnss() */
+      80             : template <int n_measurements>
+      81       81475 : void ProcTfToWorld<n_measurements>::callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg) {
+      82             : 
+      83       81475 :   if (!is_initialized_) {
+      84       81352 :     return;
+      85             :   }
+      86             : 
+      87       81475 :   if (got_gnss_) {
+      88       81352 :     return;
+      89             :   }
+      90             : 
+      91          80 :   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          81 :   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         162 :   std::scoped_lock lock(mtx_gnss_);
+     102          81 :   mrs_lib::UTM(msg->latitude, msg->longitude, &gnss_x_, &gnss_y_);
+     103          81 :   ROS_INFO("[%s]: First GNSS obtained: %.2f %.2f", Processor<n_measurements>::getPrintName().c_str(), gnss_x_, gnss_y_);
+     104          81 :   got_gnss_ = true;
+     105             : }
+     106             : /*//}*/
+     107             : 
+     108             : /*//{ process() */
+     109             : template <int n_measurements>
+     110      170665 : std::tuple<bool, bool> ProcTfToWorld<n_measurements>::process(measurement_t& measurement) {
+     111             : 
+     112      170665 :   if (!Processor<n_measurements>::enabled_) {
+     113           0 :     return {true, true};
+     114             :   }
+     115             : 
+     116      341205 :   std::scoped_lock lock(mtx_gnss_);
+     117             : 
+     118      170682 :   if (!got_gnss_) {
+     119        7458 :     ROS_WARN_THROTTLE(1.0, "[%s]: Missing GNSS data on topic: %s", Processor<n_measurements>::getPrintName().c_str(), gnss_topic_.c_str());
+     120        7458 :     return {false, false};
+     121             :   }
+     122             : 
+     123      163204 :   if (!is_gnss_offset_calculated_) {
+     124             : 
+     125          81 :     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          81 :     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          81 :     gnss_x_                    = (gnss_x_ - measurement(0)) - Processor<n_measurements>::ch_->world_origin.x;
+     130          81 :     gnss_y_                    = (gnss_y_ - measurement(1)) - Processor<n_measurements>::ch_->world_origin.y;
+     131          81 :     is_gnss_offset_calculated_ = true;
+     132          81 :     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      163224 :   measurement(0) += gnss_x_;
+     136      163115 :   measurement(1) += gnss_y_;
+     137             : 
+     138      163186 :   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      163300 :   return {true, true};
+     145             : }
+     146             : /*//}*/
+     147             : 
+     148             : /*//{ reset() */
+     149             : template <int n_measurements>
+     150           0 : void ProcTfToWorld<n_measurements>::reset() {
+     151           0 :   got_gnss_                  = false;
+     152           0 :   is_gnss_offset_calculated_ = false;
+     153           0 : }
+     154             : /*//}*/
+     155             : }  // namespace mrs_uav_state_estimators
+     156             : 
+     157             : #endif  // PROCESSORS_PROC_TF_TO_WORLD_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.overview.html new file mode 100644 index 0000000000..c44e2f6583 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.overview.html @@ -0,0 +1,60 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/proc_tf_to_world.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..a324c1257ee9037e3fabf82919209845d495e4b0 GIT binary patch literal 777 zcmV+k1NQuhP)YeKPLfI%d(hdQDZ?M6>}3P z7RRBNhCIV~)54mKVqgP#1>TJ!GMcesrS7XLRhvTlJ-0ACX3Ubb8gmW3ep|KBYVL9} zHd-<7`;=p5I<1j5^pje73r1fJMaKwP|cqki8fe9uJfx;%Wbp_7N?c1i- zs@QF_{E-s~SCSwed~Wz?gRvB~){V2)?OgR8#$YEf;2a}>+b`Au2g(_3%}QGU_x&fx z;r8R_b#C!I{JIMSxOYYEuRb6FCn{HKGMaFJ=!Hy?0as+k1)&-66k6a>E;AEZ;L4wF zan%6)Zj})NV?%eTdI$umGoG|`1`<#x$Y`sjMpN*{;DWZp2<+Wtz^zY|f%0-py;1ik zkBLUwjN#i&Dm@6@IIgQKKx!36Tpq`0W6_nq&_R}6o3E|AfdR&8jjrC4u{Ct0GH1mn@RJ^unROSIn z8T0jue2Ov0(yxHhqK~ZpCV|9C+el>18JoPqcO>uJM{*ssdR4s?ih)8QW#%Gdft*0R z<-$p4uEB(~JE*Wz;QNNbE~)Wo;y-Qt-3Y9U&PVx|$~D((KRkav&y1%T`8*R}J(;}9 z#;$y-)Ai$Mtwhw$s5(I5?6E7^s^ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - processor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71070.0 %
Date:2024-04-09 22:03:33Functions: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&)85
mrs_uav_state_estimators::Processor<2>::getNamespacedName[abi:cxx11]() const88
mrs_uav_state_estimators::Processor<1>::getPrintName[abi:cxx11]() const141
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&)203
mrs_uav_state_estimators::Processor<1>::getNamespacedName[abi:cxx11]() const271
mrs_uav_state_estimators::Processor<2>::getPrintName[abi:cxx11]() const440
+
+
+ + + +
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..573b406452 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.func.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - processor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71070.0 %
Date:2024-04-09 22:03:33Functions: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&)203
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&)85
mrs_uav_state_estimators::Processor<1>::getPrintName[abi:cxx11]() const141
mrs_uav_state_estimators::Processor<1>::getNamespacedName[abi:cxx11]() const271
mrs_uav_state_estimators::Processor<2>::getPrintName[abi:cxx11]() const440
mrs_uav_state_estimators::Processor<2>::getNamespacedName[abi:cxx11]() const88
+
+
+ + + +
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..3e7c9d7ec9 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.html @@ -0,0 +1,156 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors - processor.h (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:71070.0 %
Date:2024-04-09 22:03:33Functions: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         288 :   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         288 :       : correction_name_(correction_name), name_(name), ch_(ch), ph_(ph) {
+      30         288 :   }  // 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         359 : std::string Processor<n_measurements>::getNamespacedName() const {
+      52         359 :   return correction_name_ + "/" + name_;
+      53             : }
+      54             : /*//}*/
+      55             : 
+      56             : /*//{ getPrintName() */
+      57             : template <int n_measurements>
+      58         581 : std::string Processor<n_measurements>::getPrintName() const {
+      59         581 :   return ch_->nodelet_name + "/" + correction_name_ + "/" + name_;
+      60             : }
+      61             : /*//}*/
+      62             : 
+      63             : /*//{ toggle() */
+      64             : template <int n_measurements>
+      65           0 : void Processor<n_measurements>::toggle(bool enable) {
+      66           0 :   enabled_ = enable;
+      67           0 : }
+      68             : /*//}*/
+      69             : 
+      70             : }  // namespace mrs_uav_state_estimators
+      71             : 
+      72             : #endif  // PROCESSORS_PROCESSOR_H
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.overview.html b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.overview.html new file mode 100644 index 0000000000..804fb09cf2 --- /dev/null +++ b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.overview.html @@ -0,0 +1,38 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.png b/mrs_uav_state_estimators/include/mrs_uav_state_estimators/processors/processor.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..4a4f97a2d3dc93342019c73afac4a3056273be23 GIT binary patch literal 436 zcmV;l0ZaagP)h`Wz7!Rbg$du*y>R3?p)aIa0!P*vNNhPKFvCxC5v{Rl z)_t110j;ZuETrnnuHU&viBrZ79BzXdKV9)a-f^E-ve$EC5zK@+k?0DgW0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/agl - garmin_agl.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7510472.1 %
Date:2024-04-09 22:03:33Functions: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()61
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&)61
mrs_uav_state_estimators::garmin_agl::GarminAgl::start()61
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerCheckHealth(ros::TimerEvent const&)1257
mrs_uav_state_estimators::garmin_agl::GarminAgl::timerUpdate(ros::TimerEvent const&)122028
mrs_uav_state_estimators::garmin_agl::GarminAgl::getUavAglHeight() const128120
+
+
+ + + +
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..5c9e77832d --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/agl/garmin_agl.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/agl - garmin_agl.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:7510472.1 %
Date:2024-04-09 22:03:33Functions:61060.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

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

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

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

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

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
garmin_agl.cpp +
72.1%72.1%
+ 		72.1 %75 / 10460.0 %6 / 10
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func-sort-c.html new file mode 100644 index 0000000000..d3b9717202 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func-sort-c.html @@ -0,0 +1,212 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitude - alt_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21339553.9 %
Date:2024-04-09 22:03:33Functions: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&)146
mrs_uav_state_estimators::AltGeneric::isConverged()146
mrs_uav_state_estimators::AltGeneric::callbackReconfigure(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)146
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&)215
mrs_uav_state_estimators::AltGeneric::setInputCoeff(double const&)278
mrs_uav_state_estimators::AltGeneric::getNamespacedName[abi:cxx11]() const945
mrs_uav_state_estimators::AltGeneric::getPrintName[abi:cxx11]() const1147
mrs_uav_state_estimators::AltGeneric::start()3606
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) const133115
mrs_uav_state_estimators::AltGeneric::getState(int const&, int const&) const133143
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&) const166838
mrs_uav_state_estimators::AltGeneric::getCovarianceMatrix() const266946
mrs_uav_state_estimators::AltGeneric::getStates() const267187
mrs_uav_state_estimators::AltGeneric::setDt(double const&)267209
mrs_uav_state_estimators::AltGeneric::generateB()267606
mrs_uav_state_estimators::AltGeneric::generateA()267617
mrs_uav_state_estimators::AltGeneric::timerUpdate(ros::TimerEvent const&)307670
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) const415748
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&)415777
mrs_uav_state_estimators::AltGeneric::doCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)415785
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&) const577659
mrs_uav_state_estimators::AltGeneric::getState(int const&) const996411
+
+
+ + + +
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..2af3d2a049 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.func.html @@ -0,0 +1,212 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitude - alt_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21339553.9 %
Date:2024-04-09 22:03:33Functions: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&)146
mrs_uav_state_estimators::AltGeneric::isConverged()146
mrs_uav_state_estimators::AltGeneric::timerUpdate(ros::TimerEvent const&)307670
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&)415777
mrs_uav_state_estimators::AltGeneric::doCorrection(mrs_uav_state_estimators::Correction<1>::MeasurementStamped const&, double, mrs_uav_managers::estimation_manager::StateId_t const&)415785
mrs_uav_state_estimators::AltGeneric::setInputCoeff(double const&)278
mrs_uav_state_estimators::AltGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::AltGeneric::callbackReconfigure(mrs_uav_state_estimators::AltitudeEstimatorConfig&, unsigned int)146
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&)267209
mrs_uav_state_estimators::AltGeneric::start()3606
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&)215
mrs_uav_state_estimators::AltGeneric::setState(double const&, int const&, int const&)0
mrs_uav_state_estimators::AltGeneric::generateA()267617
mrs_uav_state_estimators::AltGeneric::generateB()267606
mrs_uav_state_estimators::AltGeneric::setStates(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)0
mrs_uav_state_estimators::AltGeneric::getPrintName[abi:cxx11]() const1147
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&) const577659
mrs_uav_state_estimators::AltGeneric::getCovariance(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&) const166838
mrs_uav_state_estimators::AltGeneric::getInnovation(int const&, int const&) const0
mrs_uav_state_estimators::AltGeneric::getNamespacedName[abi:cxx11]() const945
mrs_uav_state_estimators::AltGeneric::getCovarianceMatrix() const266946
mrs_uav_state_estimators::AltGeneric::getState(int const&) const996411
mrs_uav_state_estimators::AltGeneric::getState(int const&, int const&) const133143
mrs_uav_state_estimators::AltGeneric::getStates() const267187
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) const415748
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) const133115
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..9f8d440cc8 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.html @@ -0,0 +1,839 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitude - alt_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21339553.9 %
Date:2024-04-09 22:03:33Functions: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         146 : void AltGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16         146 :   ch_ = ch;
+      17         146 :   ph_ = ph;
+      18             : 
+      19         146 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      20             : 
+      21             :   // clang-format off
+      22         146 :   dt_ = 0.01;
+      23         146 :   input_coeff_ = 10;
+      24         146 :   default_input_coeff_ = 10;
+      25             : 
+      26         146 :   generateA();
+      27         146 :   generateB();
+      28             : 
+      29         146 :   H_ <<
+      30         146 :     1, 0, 0;
+      31             : 
+      32         146 :   innovation_ << 
+      33             :     0;
+      34             : 
+      35             :   // clang-format on
+      36             : 
+      37             :   // | --------------- initialize parameter loader -------------- |
+      38             : 
+      39         146 :   if (is_core_plugin_) {
+      40             : 
+      41         146 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      42         146 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      43             :   }
+      44             : 
+      45         146 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      46             : 
+      47             :   // | --------------------- load parameters -------------------- |
+      48             : 
+      49         146 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      50         146 :   ph->param_loader->loadParam("innovation/limit", pos_innovation_limit_);
+      51         146 :   ph->param_loader->loadParam("innovation/action", exc_innovation_action_name_);
+      52         146 :   exc_innovation_action_ = map_exc_inno_action.at(exc_innovation_action_name_);
+      53         146 :   ph->param_loader->loadParam("repredictor/enabled", is_repredictor_enabled_);
+      54         146 :   if (is_repredictor_enabled_) {
+      55           0 :     ph->param_loader->loadParam("repredictor/buffer_size", rep_buffer_size_);
+      56             :   }
+      57             : 
+      58             :   // | --------------- corrections initialization --------------- |
+      59         146 :   ph->param_loader->loadParam("corrections", correction_names_);
+      60             : 
+      61         361 :   for (auto corr_name : correction_names_) {
+      62         215 :     corrections_.push_back(std::make_shared<Correction<alt_generic::n_measurements>>(
+      63      133545 :         nh, getNamespacedName(), corr_name, ns_frame_id_, EstimatorType_t::ALTITUDE, ch_, ph_, [this](int a, int b) { return this->getState(a, b); },
+      64      415748 :         [this](const Correction<alt_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t state) {
+      65      415748 :           return this->doCorrection(meas, R, state);
+      66             :         }));
+      67             :   }
+      68             : 
+      69         146 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      70             : 
+      71             :   // | ----------- initialize process noise covariance ---------- |
+      72         146 :   Q_ = Q_t::Zero();
+      73             :   double tmp_noise;
+      74         146 :   ph->param_loader->loadParam("process_noise/pos", tmp_noise);
+      75         146 :   Q_(POSITION, POSITION) = tmp_noise;
+      76         146 :   ph->param_loader->loadParam("process_noise/vel", tmp_noise);
+      77         146 :   Q_(VELOCITY, VELOCITY) = tmp_noise;
+      78         146 :   ph->param_loader->loadParam("process_noise/acc", tmp_noise);
+      79         146 :   Q_(ACCELERATION, ACCELERATION) = tmp_noise;
+      80             : 
+      81             :   // | ------- check if all parameters loaded successfully ------ |
+      82         146 :   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         146 :       std::make_unique<drmgr_t>(ros::NodeHandle("~/" + getNamespacedName()), true, getPrintName(), boost::bind(&AltGeneric::callbackReconfigure, this, _1, _2));
+      90         146 :   drmgr_->config.pos = Q_(POSITION, POSITION);
+      91         146 :   drmgr_->config.vel = Q_(VELOCITY, VELOCITY);
+      92         146 :   drmgr_->config.acc = Q_(ACCELERATION, ACCELERATION);
+      93         146 :   drmgr_->update_config(drmgr_->config);
+      94             : 
+      95             :   // | --------------- Kalman filter intialization -------------- |
+      96         146 :   const x_t        x0 = x_t::Zero();
+      97         146 :   const P_t        P0 = 1e3 * P_t::Identity();
+      98         146 :   const statecov_t sc0({x0, P0});
+      99         146 :   sc_ = sc0;
+     100             : 
+     101         146 :   lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
+     102         146 :   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         146 :   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         292 :   mrs_lib::SubscribeHandlerOptions shopts;
+     120         146 :   shopts.nh                 = nh;
+     121         146 :   shopts.node_name          = getPrintName();
+     122         146 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     123         146 :   shopts.threadsafe         = true;
+     124         146 :   shopts.autostart          = true;
+     125         146 :   shopts.queue_size         = 10;
+     126         146 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     127             : 
+     128         146 :   sh_control_input_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimatorInput>(shopts, "control_input_in");
+     129             : 
+     130             :   // | ---------------- publishers initialization --------------- |
+     131         146 :   if (ch_->debug_topics.input) {
+     132         146 :     ph_input_ = mrs_lib::PublisherHandler<mrs_msgs::Float64ArrayStamped>(nh, getNamespacedName() + "/input", 10);
+     133             :   }
+     134         146 :   if (ch_->debug_topics.output) {
+     135         146 :     ph_output_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>(nh, getNamespacedName() + "/output", 10);
+     136             :   }
+     137         146 :   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         146 :   if (changeState(INITIALIZED_STATE)) {
+     144         146 :     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         146 : }
+     149             : /*//}*/
+     150             : 
+     151             : /*//{ start() */
+     152        3606 : bool AltGeneric::start(void) {
+     153             : 
+     154        3606 :   if (isInState(READY_STATE)) {
+     155             :     /* timer_update_.start(); */
+     156         146 :     changeState(STARTED_STATE);
+     157         146 :     return true;
+     158             : 
+     159             :   } else {
+     160        3460 :     ROS_WARN_THROTTLE(1.0, "[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+     161        3460 :     return false;
+     162             :   }
+     163             : }
+     164             : /*//}*/
+     165             : 
+     166             : /*//{ pause() */
+     167           0 : bool AltGeneric::pause(void) {
+     168             : 
+     169           0 :   if (isInState(RUNNING_STATE)) {
+     170           0 :     changeState(STOPPED_STATE);
+     171           0 :     return true;
+     172             : 
+     173             :   } else {
+     174           0 :     return false;
+     175             :   }
+     176             : }
+     177             : /*//}*/
+     178             : 
+     179             : /*//{ reset() */
+     180           0 : bool AltGeneric::reset(void) {
+     181             : 
+     182           0 :   if (!isInitialized()) {
+     183           0 :     ROS_ERROR("[%s]: Cannot reset uninitialized estimator", getPrintName().c_str());
+     184           0 :     return false;
+     185             :   }
+     186             : 
+     187           0 :   changeState(STOPPED_STATE);
+     188             : 
+     189             :   // Initialize LKF state and covariance
+     190           0 :   const x_t        x0 = x_t::Zero();
+     191           0 :   const P_t        P0 = 1e6 * P_t::Identity();
+     192           0 :   const statecov_t sc0({x0, P0});
+     193           0 :   sc_ = sc0;
+     194             : 
+     195             :   // Instantiate the LKF itself
+     196           0 :   lkf_ = std::make_shared<lkf_t>(A_, B_, H_);
+     197           0 :   if (is_repredictor_enabled_) {
+     198             : 
+     199           0 :     const u_t       u0 = u_t::Zero();
+     200           0 :     const ros::Time t0 = ros::Time(0);
+     201           0 :     lkf_rep_           = std::make_unique<mrs_lib::Repredictor<varstep_lkf_t>>(x0, P0, u0, Q_, t0, models_.at(0), rep_buffer_size_);
+     202             :   }
+     203             : 
+     204           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     205             : 
+     206           0 :   return true;
+     207             : }
+     208             : /*//}*/
+     209             : 
+     210             : /* timerUpdate() //{*/
+     211      307670 : void AltGeneric::timerUpdate(const ros::TimerEvent &event) {
+     212             : 
+     213             : 
+     214      307670 :   if (!isInitialized()) {
+     215       40398 :     return;
+     216             :   }
+     217             : 
+     218      307561 :   switch (getCurrentSmState()) {
+     219             : 
+     220           0 :     case UNINITIALIZED_STATE: {
+     221           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     222           0 :       break;
+     223             :     }
+     224             : 
+     225        4980 :     case READY_STATE: {
+     226        4980 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     227        4980 :       break;
+     228             :     }
+     229             : 
+     230        7800 :     case INITIALIZED_STATE: {
+     231             :       // initialize the estimator with current corrections
+     232        8015 :       for (auto correction : corrections_) {
+     233        7857 :         auto res = correction->getProcessedCorrection();
+     234        7848 :         if (res) {
+     235         215 :           auto measurement_stamped = res.value();
+     236         215 :           setState(measurement_stamped.value(0), correction->getStateId());
+     237         215 :           ROS_INFO("[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(0));
+     238             :         } else {
+     239        7637 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(),
+     240             :                             correction->getNamespacedName().c_str());
+     241        7642 :           return;
+     242             :         }
+     243             :       }
+     244         145 :       changeState(READY_STATE);
+     245         146 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     246         146 :       break;
+     247             :     }
+     248             : 
+     249         146 :     case STARTED_STATE: {
+     250         146 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     251             : 
+     252         146 :       if (isConverged()) {
+     253         146 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     254         146 :         changeState(RUNNING_STATE);
+     255             :       }
+     256         146 :       break;
+     257             :     }
+     258             : 
+     259      294765 :     case RUNNING_STATE: {
+     260      735187 :       for (auto correction : corrections_) {
+     261      440317 :         if (!correction->isHealthy()) {
+     262           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     263           0 :           changeState(ERROR_STATE);
+     264             :         }
+     265             :       }
+     266      294797 :       break;
+     267             :     }
+     268             : 
+     269           0 :     case STOPPED_STATE: {
+     270           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     271           0 :       break;
+     272             :     }
+     273             : 
+     274           0 :     case ERROR_STATE: {
+     275           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     276           0 :       ros::Time t_now = ros::Time::now();
+     277           0 :       if (is_error_state_first_time_) {
+     278           0 :         prev_time_in_error_state_  = t_now;
+     279           0 :         is_error_state_first_time_ = false;
+     280           0 :         error_state_duration_      = ros::Duration(0.0);
+     281             :       }
+     282           0 :       error_state_duration_ += t_now - prev_time_in_error_state_;
+     283             : 
+     284             : 
+     285             :       // check if all corrections are healthy now
+     286           0 :       bool all_corrections_healthy = true;
+     287           0 :       for (auto correction : corrections_) {
+     288           0 :         if (!correction->isHealthy()) {
+     289           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     290           0 :           all_corrections_healthy = false;
+     291             :         }
+     292             :       }
+     293             : 
+     294           0 :       if (all_corrections_healthy && innovation_ok_) {
+     295             :         // initialize the estimator again if corrections become healthy
+     296           0 :         if (error_state_duration_.toSec() > 5.0) {
+     297           0 :           ROS_INFO("[%s]: corrections healthy for %.2f s", getPrintName().c_str(), error_state_duration_.toSec());
+     298           0 :           changeState(INITIALIZED_STATE);
+     299           0 :           is_error_state_first_time_ = true;
+     300           0 :         }
+     301             :       } else {
+     302           0 :         is_error_state_first_time_ = true;
+     303             :       }
+     304             : 
+     305           0 :       prev_time_in_error_state_ = t_now;
+     306             : 
+     307           0 :       break;
+     308             :     }
+     309             :   }
+     310             : 
+     311      300087 :   if (sh_control_input_.newMsg()) {
+     312      134798 :     is_input_ready_ = true;
+     313             :   }
+     314             : 
+     315             :   // check age of input
+     316      299759 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     317          22 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f), using zero input instead", getPrintName().c_str(),
+     318             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     319          22 :     is_input_ready_ = false;
+     320             :   }
+     321             : 
+     322      299584 :   if (!isRunning() && !isStarted()) {
+     323        5121 :     return;
+     324             :   }
+     325             : 
+     326      294615 :   if (first_iter_) {
+     327         146 :     first_iter_ = false;
+     328         146 :     return;
+     329             :   }
+     330             : 
+     331      294469 :   double dt = (event.current_real - event.last_real).toSec();
+     332      294505 :   if (dt <= 0.0) {
+     333       27490 :     return;
+     334             :   }
+     335             : 
+     336      267015 :   if (!is_repredictor_enabled_) {  // repredictor calculates dt on its own
+     337      266423 :     setDt(dt);
+     338             :   }
+     339             : 
+     340             :   // prediction step
+     341      267397 :   u_t       u;
+     342      266488 :   ros::Time input_stamp;
+     343      266664 :   if (is_input_ready_) {
+     344      155334 :     mrs_msgs::EstimatorInputConstPtr msg            = sh_control_input_.getMsg();
+     345      155403 :     const tf2::Vector3               des_acc_global = getAccGlobal(msg, 0);  // we don't care about heading
+     346      155811 :     input_stamp                                     = msg->header.stamp;
+     347      155212 :     if (input_coeff_ != default_input_coeff_){
+     348         110 :       setInputCoeff(default_input_coeff_);
+     349             :     }
+     350      155212 :     u(0) = des_acc_global.getZ();
+     351             :   } else {
+     352      110748 :     ROS_DEBUG_THROTTLE(1.0, "[%s]: not receiving control input, estimation suboptimal, potentially unstable", getPrintName().c_str());
+     353      111289 :     input_stamp = ros::Time::now();
+     354      111415 :     if (input_coeff_ != 0){
+     355         168 :       setInputCoeff(0);
+     356             :     }
+     357      111415 :     u = u_t::Zero();
+     358             :   }
+     359             : 
+     360             :   // go through available corrections and apply them
+     361             :   /* for (auto correction : corrections_) { */
+     362             :   /*   auto res = correction->getProcessedCorrection(); */
+     363             :   /*   if (res) { */
+     364             :   /*     auto measurement_stamped = res.value(); */
+     365             :   /*     doCorrection(measurement_stamped.value, correction->getR(), correction->getStateId(), measurement_stamped.stamp); */
+     366             :   /*   } */
+     367             :   /* } */
+     368             : 
+     369      265618 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     370      266243 :   Q_t        Q  = mrs_lib::get_mutexed(mtx_Q_, Q_);
+     371             : 
+     372             :   try {
+     373             :     // Apply the prediction step
+     374      532420 :     std::scoped_lock lock(mutex_lkf_);
+     375      266113 :     if (is_repredictor_enabled_) {
+     376           0 :       lkf_rep_->addInputChangeWithNoise(u, Q, input_stamp, models_[0]);
+     377           0 :       sc = lkf_rep_->predictTo(ros::Time::now());
+     378             :     } else {
+     379      266113 :       sc = lkf_->predict(sc, u, Q, dt_);
+     380             :     }
+     381             :   }
+     382           0 :   catch (const std::exception &e) {
+     383             :     // In case of error, alert the user
+     384           0 :     ROS_ERROR("[%s]: LKF prediction failed: %s", getPrintName().c_str(), e.what());
+     385             :   }
+     386             : 
+     387      264384 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     388             : 
+     389             :   // publishing
+     390      265671 :   publishInput(u, input_stamp);
+     391      267238 :   publishOutput();
+     392      267289 :   publishDiagnostics();
+     393             : }
+     394             : /*//}*/
+     395             : 
+     396             : /*//{ timerCheckHealth() */
+     397           0 : void AltGeneric::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     398             : 
+     399           0 :   if (!isInitialized()) {
+     400           0 :     return;
+     401             :   }
+     402             : 
+     403           0 :   switch (getCurrentSmState()) {
+     404             : 
+     405           0 :     case UNINITIALIZED_STATE: {
+     406           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s initialization", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     407           0 :       break;
+     408             :     }
+     409             : 
+     410           0 :     case READY_STATE: {
+     411           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s estimator start", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     412           0 :       break;
+     413             :     }
+     414             : 
+     415           0 :     case INITIALIZED_STATE: {
+     416             : 
+     417             :       // initialize the estimator with current corrections
+     418           0 :       for (auto correction : corrections_) {
+     419           0 :         auto res = correction->getProcessedCorrection();
+     420           0 :         if (res) {
+     421           0 :           auto measurement_stamped = res.value();
+     422           0 :           setState(measurement_stamped.value(0), correction->getStateId());
+     423           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(0));
+     424             :         } else {
+     425           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(),
+     426             :                             correction->getNamespacedName().c_str());
+     427           0 :           return;
+     428             :         }
+     429             :       }
+     430           0 :       changeState(READY_STATE);
+     431           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     432           0 :       break;
+     433             :     }
+     434             : 
+     435           0 :     case STARTED_STATE: {
+     436           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     437             : 
+     438           0 :       if (isConverged()) {
+     439           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: LKF converged", getPrintName().c_str());
+     440           0 :         changeState(RUNNING_STATE);
+     441             :       }
+     442           0 :       break;
+     443             :     }
+     444             : 
+     445           0 :     case RUNNING_STATE: {
+     446           0 :       for (auto correction : corrections_) {
+     447           0 :         if (!correction->isHealthy()) {
+     448           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     449           0 :           changeState(ERROR_STATE);
+     450             :         }
+     451             :       }
+     452           0 :       break;
+     453             :     }
+     454             : 
+     455           0 :     case STOPPED_STATE: {
+     456           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is stopped", getPrintName().c_str());
+     457           0 :       break;
+     458             :     }
+     459             : 
+     460           0 :     case ERROR_STATE: {
+     461           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is in ERROR state", getPrintName().c_str());
+     462           0 :       bool all_corrections_healthy = true;
+     463           0 :       for (auto correction : corrections_) {
+     464           0 :         if (!correction->isHealthy()) {
+     465           0 :           ROS_ERROR_THROTTLE(1.0, "[%s]: Correction %s is not healthy!", getPrintName().c_str(), correction->getNamespacedName().c_str());
+     466           0 :           all_corrections_healthy = false;
+     467             :         }
+     468             :       }
+     469             :       // initialize the estimator again if corrections become healthy
+     470           0 :       if (all_corrections_healthy) {
+     471           0 :         changeState(INITIALIZED_STATE);
+     472             :       }
+     473           0 :       break;
+     474             :     }
+     475             :   }
+     476             : 
+     477           0 :   if (sh_control_input_.newMsg()) {
+     478           0 :     is_input_ready_ = true;
+     479             :   }
+     480             : 
+     481             :   // check age of input
+     482           0 :   if (is_input_ready_ && (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec() > 0.1) {
+     483           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: input too old (%.4f), using zero input instead", getPrintName().c_str(),
+     484             :                       (ros::Time::now() - sh_control_input_.lastMsgTime()).toSec());
+     485           0 :     is_input_ready_ = false;
+     486             :   }
+     487             : }
+     488             : /*//}*/
+     489             : 
+     490             : /*//{ doCorrection() */
+     491      415785 : void AltGeneric::doCorrection(const Correction<alt_generic::n_measurements>::MeasurementStamped &meas, const double R, const StateId_t &state_id) {
+     492      415785 :   doCorrection(meas.value, R, state_id, meas.stamp);
+     493      415418 : }
+     494             : /*//}*/
+     495             : 
+     496             : /*//{ doCorrection() */
+     497      415777 : void AltGeneric::doCorrection(const z_t &z, const double R, const StateId_t &H_idx, const ros::Time &meas_stamp) {
+     498             : 
+     499      415777 :   if (!isInitialized()) {
+     500        3769 :     return;
+     501             :   }
+     502             : 
+     503             :   // we do not want to perform corrections until the estimator is initialized
+     504      415705 :   if (!(isInState(SMStates_t::READY_STATE) || isInState(SMStates_t::RUNNING_STATE) || isInState(SMStates_t::STARTED_STATE))) {
+     505        3769 :     return;
+     506             :   }
+     507             : 
+     508             :   // for position state check the innovation
+     509      411781 :   if (H_idx == POSITION) {
+     510      240748 :     std::scoped_lock lock(mtx_innovation_);
+     511             : 
+     512      240827 :     is_mitigating_jump_ = false;
+     513      240910 :     innovation_(0)      = z(0) - getState(POSITION);
+     514             : 
+     515      240842 :     if (fabs(innovation_(0)) > pos_innovation_limit_) {
+     516           0 :       ROS_WARN_THROTTLE(1.0, "[%s]: innovation too large - [%.2f] lim: %.2f", getPrintName().c_str(), innovation_(0), pos_innovation_limit_);
+     517           0 :       innovation_ok_ = false;
+     518           0 :       switch (exc_innovation_action_) {
+     519           0 :         case ExcInnoAction_t::ELAND: {
+     520           0 :           ROS_WARN_THROTTLE(1.0, "[%s]: large innovation should trigger eland in control manager", ros::this_node::getName().c_str());
+     521           0 :           changeState(ERROR_STATE);
+     522           0 :           break;
+     523             :         }
+     524           0 :         case ExcInnoAction_t::SWITCH: {
+     525           0 :           ROS_WARN_THROTTLE(1.0, "[%s]: innovation should trigger estimator switch but no eland", ros::this_node::getName().c_str());
+     526           0 :           innovation_(0) = 0.0;  // this is quite hacky but is there other way to switch estimators and not trigger eland by the large innovation?
+     527           0 :           changeState(ERROR_STATE);
+     528           0 :           break;
+     529             :         }
+     530           0 :         case ExcInnoAction_t::MITIGATE: {
+     531           0 :           ROS_WARN_THROTTLE(1.0, "[%s]: large innovation should trigger estimate jump mitigation", ros::this_node::getName().c_str());
+     532           0 :           innovation_(0)      = 0.0;  // this is quite hacky but is there other way to switch estimators and not trigger eland by the large innovation?
+     533           0 :           is_mitigating_jump_ = true;
+     534           0 :           setState(z(0), POSITION);
+     535           0 :           break;
+     536             :         }
+     537           0 :         case ExcInnoAction_t::NONE: {
+     538           0 :           ROS_WARN_THROTTLE(1.0, "[%s]: large innovation ignored", ros::this_node::getName().c_str());
+     539           0 :           break;
+     540             :         }
+     541             :       }
+     542             :     }
+     543      240851 :     innovation_ok_ = true;
+     544             :   }
+     545             : 
+     546      411886 :   statecov_t sc = mrs_lib::get_mutexed(mutex_sc_, sc_);
+     547             :   try {
+     548             :     // Apply the correction step
+     549             :     {
+     550      822056 :       std::scoped_lock lock(mutex_lkf_);
+     551      411982 :       H_        = H_t::Zero();
+     552      411892 :       H_(H_idx) = 1;
+     553      411324 :       lkf_->H   = H_;
+     554      410606 :       if (is_repredictor_enabled_) {
+     555             : 
+     556           0 :         lkf_rep_->addMeasurement(z, R_t::Ones() * R, meas_stamp, models_[H_idx]);
+     557             :       } else {
+     558      410606 :         sc = lkf_->correct(sc, z, R_t::Ones() * R);
+     559             :       }
+     560             :     }
+     561             :   }
+     562           0 :   catch (const std::exception &e) {
+     563             :     // In case of error, alert the user
+     564           0 :     ROS_ERROR("[%s]: LKF correction failed: %s", getPrintName().c_str(), e.what());
+     565             :   }
+     566             : 
+     567      408606 :   mrs_lib::set_mutexed(mutex_sc_, sc, sc_);
+     568             : }  // namespace mrs_uav_state_estimators
+     569             : /*//}*/
+     570             : 
+     571             : /*//{ isConverged() */
+     572         146 : bool AltGeneric::isConverged() {
+     573             : 
+     574             :   // TODO: check convergence by rate of change of determinant
+     575             : 
+     576         146 :   return true;
+     577             : }
+     578             : /*//}*/
+     579             : 
+     580             : /*//{ getState() */
+     581      133143 : double AltGeneric::getState(const int &state_id_in, const int &axis_in) const {
+     582      133143 :   return getState(stateIdToIndex(state_id_in, axis_in));
+     583             : }
+     584             : 
+     585      996411 : double AltGeneric::getState(const int &state_idx_in) const {
+     586      996411 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x(state_idx_in);
+     587             : }
+     588             : /*//}*/
+     589             : 
+     590             : /*//{ setState() */
+     591           0 : void AltGeneric::setState(const double &state_in, const int &state_id_in, const int &axis_in) {
+     592           0 :   setState(state_in, stateIdToIndex(state_id_in, axis_in));
+     593           0 : }
+     594             : 
+     595         215 : void AltGeneric::setState(const double &state_in, const int &state_idx_in) {
+     596         215 :   mrs_lib::set_mutexed(mutex_sc_, state_in, sc_.x(state_idx_in));
+     597         215 : }
+     598             : /*//}*/
+     599             : 
+     600             : /*//{ getStates() */
+     601      267187 : AltGeneric::states_t AltGeneric::getStates(void) const {
+     602      534082 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).x;
+     603             : }
+     604             : /*//}*/
+     605             : 
+     606             : /*//{ setStates() */
+     607           0 : void AltGeneric::setStates(const states_t &states_in) {
+     608           0 :   mrs_lib::set_mutexed(mutex_sc_, states_in, sc_.x);
+     609           0 : }
+     610             : /*//}*/
+     611             : 
+     612             : /*//{ getCovariance() */
+     613           0 : double AltGeneric::getCovariance(const int &state_id_in, const int &axis_in) const {
+     614           0 :   return getCovariance(stateIdToIndex(state_id_in, axis_in));
+     615             : }
+     616             : 
+     617      577659 : double AltGeneric::getCovariance(const int &state_idx_in) const {
+     618      577659 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P(state_idx_in, state_idx_in);
+     619             : }
+     620             : /*//}*/
+     621             : 
+     622             : /*//{ getCovarianceMatrix() */
+     623      266946 : AltGeneric::covariance_t AltGeneric::getCovarianceMatrix(void) const {
+     624      533698 :   return mrs_lib::get_mutexed(mutex_sc_, sc_).P;
+     625             : }
+     626             : /*//}*/
+     627             : 
+     628             : /*//{ setCovarianceMatrix() */
+     629           0 : void AltGeneric::setCovarianceMatrix(const covariance_t &cov_in) {
+     630           0 :   mrs_lib::set_mutexed(mutex_sc_, cov_in, sc_.P);
+     631           0 : }
+     632             : /*//}*/
+     633             : 
+     634             : /*//{ getInnovation() */
+     635      166838 : double AltGeneric::getInnovation(const int &state_idx) const {
+     636      166838 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_)(state_idx);
+     637             : }
+     638             : 
+     639           0 : double AltGeneric::getInnovation(const int &state_id_in, const int &axis_in) const {
+     640           0 :   return getInnovation(stateIdToIndex(state_id_in, axis_in));
+     641             : }
+     642             : /*//}*/
+     643             : 
+     644             : /*//{ setDt() */
+     645      267209 : void AltGeneric::setDt(const double &dt) {
+     646      267209 :   dt_ = dt;
+     647      267209 :   generateA();
+     648      266849 :   generateB();
+     649      533621 :   std::scoped_lock lock(mutex_lkf_);
+     650      266644 :   lkf_->A = A_;
+     651      266814 :   lkf_->B = B_;
+     652      266493 : }
+     653             : /*//}*/
+     654             : 
+     655             : /*//{ setInputCoeff() */
+     656         278 : void AltGeneric::setInputCoeff(const double &input_coeff) {
+     657         278 :   input_coeff_ = input_coeff;
+     658         278 :   generateA();
+     659         277 :   generateB();
+     660         553 :   std::scoped_lock lock(mutex_lkf_);
+     661         277 :   lkf_->A = A_;
+     662         278 :   lkf_->B = B_;
+     663             : 
+     664         277 :   if (is_repredictor_enabled_) {
+     665           0 :     models_.clear();
+     666           0 :     generateRepredictorModels(input_coeff_);
+     667             :   }
+     668         278 : }
+     669             : /*//}*/
+     670             : 
+     671             : /*//{ generateRepredictorModels() */
+     672           0 : void AltGeneric::generateRepredictorModels(const double input_coeff) {
+     673             : 
+     674           0 :     for (int i = 0; i < alt_generic::n_states; i++) {
+     675             : 
+     676           0 :       auto lambda_generateA = [input_coeff](const double dt) {
+     677           0 :         A_t A;
+     678             :         // clang-format off
+     679           0 :         A <<
+     680           0 :           1, dt, 0.5 * dt * dt,
+     681           0 :           0, 1, dt,
+     682           0 :           0, 0, 1-(input_coeff * dt);
+     683             :         // clang-format on
+     684           0 :         return A;
+     685           0 :       };
+     686             : 
+     687           0 :       auto lambda_generateB = [input_coeff]([[maybe_unused]] const double dt) {
+     688           0 :         B_t B = B.Zero();
+     689             :         // clang-format off
+     690           0 :         B <<
+     691             :           0,
+     692           0 :           0,
+     693           0 :           (input_coeff * dt);
+     694             :         // clang-format on
+     695           0 :         return B;
+     696           0 :       };
+     697             : 
+     698           0 :       H_t H = H_t::Zero();
+     699           0 :       H(i)  = 1;
+     700           0 :       models_.push_back(std::make_shared<varstep_lkf_t>(lambda_generateA, lambda_generateB, H));
+     701             :     }
+     702           0 : }
+     703             : /*//}*/
+     704             : 
+     705             : /*//{ generateA() */
+     706      267617 : void AltGeneric::generateA() {
+     707             : 
+     708             :   // clang-format off
+     709      267617 :     A_ <<
+     710      266524 :       1, dt_, 0.5 * dt_ * dt_,
+     711      266902 :       0, 1, dt_,
+     712      267147 :       0, 0, 1-(input_coeff_ * dt_);
+     713             :   // clang-format on
+     714      266821 : }
+     715             : /*//}*/
+     716             : 
+     717             : /*//{ generateB() */
+     718      267606 : void AltGeneric::generateB() {
+     719             : 
+     720             :   // clang-format off
+     721      267606 :     B_ <<
+     722             :       0,
+     723      266409 :       0,
+     724      266699 :       (input_coeff_ * dt_);
+     725             :   // clang-format on
+     726      266536 : }
+     727             : /*//}*/
+     728             : 
+     729             : /*//{ callbackReconfigure() */
+     730         146 : void AltGeneric::callbackReconfigure(AltitudeEstimatorConfig &config, [[maybe_unused]] uint32_t level) {
+     731             : 
+     732         146 :   if (!isInitialized()) {
+     733         146 :     return;
+     734             :   }
+     735             : 
+     736           0 :   Q_t Q;
+     737           0 :   Q(POSITION, POSITION)         = config.pos;
+     738           0 :   Q(VELOCITY, VELOCITY)         = config.vel;
+     739           0 :   Q(ACCELERATION, ACCELERATION) = config.acc;
+     740           0 :   mrs_lib::set_mutexed(mtx_Q_, Q, Q_);
+     741             : }
+     742             : /*//}*/
+     743             : 
+     744             : /*//{ getNamespacedName() */
+     745         945 : std::string AltGeneric::getNamespacedName() const {
+     746        1890 :   return parent_state_est_name_ + "/" + getName();
+     747             : }
+     748             : /*//}*/
+     749             : 
+     750             : /*//{ getPrintName() */
+     751        1147 : std::string AltGeneric::getPrintName() const {
+     752        2294 :   return ch_->nodelet_name + "/" + parent_state_est_name_ + "/" + getName();
+     753             : }
+     754             : /*//}*/
+     755             : };  // namespace mrs_uav_state_estimators
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.overview.html new file mode 100644 index 0000000000..e22283543d --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.overview.html @@ -0,0 +1,209 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/altitude/alt_generic.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..a245dc5de33175ed9ba05e949e66467ab8dabd3b GIT binary patch literal 2916 zcmV-q3!C(bP)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$WrFJ+sWoy;-(t@SS>~w&X_>Atdz4u! z17<1sk^N*ISc8{>4(4&`j@jTnDX40HNzsmHBnxWh z2rj5y4eS{_dx|?aBnjWqS8-gBM8g3Bd>p{deutPcJ$8+*wiP_)B4f0Z zOCfs>IM#zmUpXUuN~I{2*wPW_8WKQ5Uk48uv;{=s+-zzR3zK7{AuQIB6)@K0nE{iZ zT`n-woGh#q7v|BK$rQm1H&KO}X%;pxiZR74tY8dhPz5lX+XU$hBLggCG(kpw&CcMq z2gocQ4iZ0koCCcdObc_UGeFJ1wt&cy)T)I5!_VItcCgpNz3gCL!|Bev<4+#!L`;#E z0x?VMZF|6lr-mie7BG$x=r#z3?Lun(6eNIhERutE?P*9qmFnotACDvm0)Mswc!lo?*q^v2Ia=Hz_ zpo1+AJ2c0F)w#-z=NAOxhb`N!StV6M#26w=h^wR`iE%EZ;r9`^>q;ET@Ntax*T2f1 zK=tVREw$#kPv%l|FEn3cm5ZMyX6H4DK`aqbr}Sty1&G)`zcZeekq1)a5z`H{GoM&v zC>PX9IfG>)dgY|pUl#!3`wFQ|@V97Udtm|NnL|t$HsQ7HLQsW#GQ#MmPAq^-m)v?O z69Yy_L(<`=_oblS5782j^b*e&hEB%FQk3S7*;72Zn31on5oS-hiIV`Y&>6aZ`DE3$ zOoxtYl*h|?3lNJ`FJF+?9};R%k076 z;NcptGWQ^Lmzbfgu@j2o>pry((fvU_52tqscbbeLtv?4ad(@02tQ%oOd#LHv42IW;{zjM7cs zeBZUs&8W$)m|-u9s{~jx_HgsuRShhfO=M$DGX+c%=13^O(b&KOh}uQveMa|5-4B1J zK>L$CZpcT+4X1Y?rc6&$GL>~9Syg*5M}kk?lKR}uvY?DP;9Nhvd?t?YDLpx2ZqIKa zNuvNbhXcfS8Z%3QoUbxQe==qH$1sG&IOOk+Z=uC4} z%BPO!g;}(=)Tf*Q&jGW!v*yQ*e|cuOP*`lK&p?15-I<$|U4F5WJAKK{Byzj{BbD6$ z>xG4p`V>9gH0&ut$cR>qWN!8pJ^ZNIJH0A}bph~bcqz1UPnSd=a8-(kp&&u>5j)YP zSjac?(HuX9BO%u@s@2R^_I&(0M>Q4}`kIxCm~ld5mT12?6j|s5ZU}EcD}< zwnwYoze(GsxgYicNEE$Mj`QJC-^x=t##}N!OpJTRVdY}Xz}u*cYs~YwKTO%N`b@D% zOMt`C5}ZG;=#8m)KR%}@&ah9SJ9ES|^XyIOvNAxWEiBkafBmX*B!wIRn_k5z+IzO* z#jg?f!;z!CscmP1sa9A{{^OGPW*f^FpYx_$&PVe5UR29ugIkzTJ=wI%WS{mE$aNe@ zFzP-{%*PM2M{&+->TTeXcYo<@I)6nSif&|Kl*EwMxeXp*D=}^~sv>kJdCz6dx2M z{KCMe)==@nYVoZF+DGuKd*@M;TirasCu7{A?po~-c_kb-p+LE5n=AZAHwZVS>Kg7+nrM^aUImgHW{JX6 zv~G~6s5X6x!j(y5K_0U;-asC!R7>LJy@gnM)_9P=oJbEWkE z!|QoXTRBuU#HogdC}qASm!%de_K_YWdIx!|ZjMYuu8d!$g;lCWA)86hyoWAoVYr*c z#q1ljupr@)(N(;ZzW5cJMI4g8EV67AQ07O=a1Kz-$ z0hCLyj>1K(~+c}p4P7hd|HU@P3Q?g+R zF*S@kCZtgkp!tj~WGA*w4Z1BnAZiU&mu%trfaUEAfZwo%OZl;i1h@h$PrGNuvCoIM z7Tv(T1`--)Ke-tH7&!2Q&Rp%o_)n&(00N%^5dfW|Q@DQVpAuIq + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/altitude + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/altitudeHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:21339553.9 %
Date:2024-04-09 22:03:33Functions:223366.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

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

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

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

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/heading/hdg_generic.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..609fc109848caebf2d99ac05317361c2b7aea3a5 GIT binary patch literal 2308 zcmV+f3H$bmP)%-Vg0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpRGaN<3|zBMI@TE#PU-5u=>d+Ns*?Q)3)Y7#U^< za!hgbl>^pVqU$;xb4&>cRCc7tofuiptm2xK!Z6lUHdm!sd93MpiVd&RA!d@Fl~ zpE}K2G?iqGhM_VdR6pyU`r7~WGZ$ta&Ya+x7U3D$m>r9PL>W*CSM(xN-$V^S7|4Gb#!04zp`=4&Bg1sW+nIVP3r^J-9 zY3?Kt!o>(+iQOy>%?X#h2|gh#gSR6%nutr1i8iM&yHvY(Y+A{a-N%0RLQcG zo3%nVF!3o@G0Mn3AIUh*dzKW%e3fPtq&Uf0&s2_(5ARDI!uyo9Cv2@L61vDyn7f}^ zC3L7Vo@wuKmkGf^rXJzv!$ZY71jS@#uL$UOE_4+(Q#;wkcFrWR)=5m)uh zb+HE?eQw5UPPEQ{gdkw6!-rdT^bZgJB1abwA6tDQ!R-jrnPo%DaAw9cDHBu)Yu#i+ zP1Vg{;1Wu?+STS1%2Kd;E;SLPRyb>>W-K7G2}3rVzq5>*!*Q6wi%i*5KNz=xnI4+& zkDdnvytAkteH^I_bVGrtGp8^V6KOD#i zPQ5ErI1UrdE;&p8uQ@AtLTGBtJ`?82$_zayD`U6zm~sbY#`<^}W8Wwz%vq0?FmKcO zvINi3PtL`>p3v%N!_<}PYGV8ijegb!WdUZ~aUp&MQ^Y2+fsU^{$JCIk+NsrGWvw~t zd5^*Hv*QAbxSVlZhi}&|g<)oYhozu}W+6yfw>24Q3HKvpkiKhS*(JEXb#4rlYwm>T zm6-1^I37}f!)CV%(>mT=nM|M8H3Hq!78k(rUQ1!C_Bpx~E>~uLzfOp#a^pZ)9f&?8oe(YiuqUj;OT*p?7g*}dNP=(OR~U$UPG7+v z*VVhgMVaDdF7@Nevo3CjE+t6?;bBxDmpMvn;5_qWt!*k>4T=Sq9rxtG)XS(^@oHsu zTNfDAh^VeLTJIF#P;tFyk}j4eg}z_m?`|j0Tp{D)*M7MHoNJa{X(iP3;zhaCb>s># z?`6NYy5`7w;uzb;x2_iM7XDO1B*kJ|)= z@T6L^?Lg7yED^Z2y$sXU_k9Bblrg3$+RCY#O#?eEZMrT2ga>W{J1I|vWSGGtQ@p|e z*>DW)ZGrIU9M_POiO=6gOTk5hq^r9UVfIWamo+7`m=>P?l!gwAJ<@tHwJ?L)UXoTfkBI)7@X8(%$tQ+8eAh!FXr=9>r-!7-`1FvT9umXx=^;Hm zq)x`|`laynkT}Aphcq_$(?fcCNIl1!0QVbuNK%k|dPqa~K0Txgy<%F_Ub76^t~!&5I|yIpB-k9+Y~ zj*s#Ub{9PZPR*3!e_M6cWlZS7Inp8*6}0m;nZV|B&TmAvWMG7F=cd$BsBs5`mn9zy z{#Ys4pMixm417d8?jN+SSmNlkcYQ@dH+blq5Pq7v#$rKrA;6;t>J7!Sf+gG%!>uDk zw_(^b1mHs%(a`+=Y&PAY_o3aq*dTn*?uGdpiO&p3lJb$BU-7_Fdhde9cWW%3UxSn@ z&Hj^X`!kt*AD?Lr4^hUZKF`WX{{3y9X_U@2m)c466->S6^zIC-HArV58FvpcK`Mi1 zqhj9SxToF@W5Ly-ae&88#v9-f(7K0LOeWgfIE=3uz3<>_Rd3dVsivAg$2X?2r-Wu1 z{)6~PWpu|Zzh%c-cglQ+*1WFCe<=X5tI%!zLi+YqSOr~a71oX6 zDXrl#kn!wDaTXru-}4a;J)Dda?_u}jctYi19|;jDal-FdTugLio<3>bXFn=6{UsNw z!WEA2*>$W0Lr4clh*lMDfky?ELUbgkOdC6@jENlx>#8EVZ3{m$97!{tK0V~w+KO;lCgB|;PcuV63@p1AXe#EQQ@Lwx4@#|2eS~*9UAwGCf zBm!^qxbRRHLQ|Wx3e}51U*tm7i(8kbwB^67^&p`k!SMRy-lydU8fxRu|JF?xD1$Tk znBvl65CxtkERd11v%8>YxM+&fHELSNw`-=nfAW@EtGB~Ct=UjfYi(_FJ*1f6Qmx}g eLmztQruYv!>Z}Ez%7c&q0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_passthrough.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9113368.4 %
Date:2024-04-09 22:03:33Functions: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&)85
mrs_uav_state_estimators::HdgPassthrough::start()147
mrs_uav_state_estimators::HdgPassthrough::getNamespacedName[abi:cxx11]() const170
mrs_uav_state_estimators::HdgPassthrough::getPrintName[abi:cxx11]() const503
mrs_uav_state_estimators::HdgPassthrough::callbackAngularVelocity(boost::shared_ptr<geometry_msgs::Vector3Stamped_<std::allocator<void> > const>)173513
mrs_uav_state_estimators::HdgPassthrough::getCovarianceMatrix() const173530
mrs_uav_state_estimators::HdgPassthrough::getStates() const173559
mrs_uav_state_estimators::HdgPassthrough::timerUpdate(ros::TimerEvent const&)173563
mrs_uav_state_estimators::HdgPassthrough::timerCheckHealth(ros::TimerEvent const&)176503
mrs_uav_state_estimators::HdgPassthrough::callbackOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)177673
mrs_uav_state_estimators::HdgPassthrough::setState(double const&, int const&)351175
mrs_uav_state_estimators::HdgPassthrough::getState(int const&) const602773
+
+
+ + + +
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..9ecd9454c7 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.func.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_passthrough.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9113368.4 %
Date:2024-04-09 22:03:33Functions: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&)85
mrs_uav_state_estimators::HdgPassthrough::timerUpdate(ros::TimerEvent const&)173563
mrs_uav_state_estimators::HdgPassthrough::timerCheckHealth(ros::TimerEvent const&)176503
mrs_uav_state_estimators::HdgPassthrough::callbackOrientation(boost::shared_ptr<geometry_msgs::QuaternionStamped_<std::allocator<void> > const>)177673
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>)173513
mrs_uav_state_estimators::HdgPassthrough::pause()0
mrs_uav_state_estimators::HdgPassthrough::reset()0
mrs_uav_state_estimators::HdgPassthrough::start()147
mrs_uav_state_estimators::HdgPassthrough::setState(double const&, int const&)351175
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]() const503
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]() const170
mrs_uav_state_estimators::HdgPassthrough::getCovarianceMatrix() const173530
mrs_uav_state_estimators::HdgPassthrough::getState(int const&) const602773
mrs_uav_state_estimators::HdgPassthrough::getState(int const&, int const&) const0
mrs_uav_state_estimators::HdgPassthrough::getStates() const173559
+
+
+ + + +
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..23c60dc2ae --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.html @@ -0,0 +1,391 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/heading - hdg_passthrough.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:9113368.4 %
Date:2024-04-09 22:03:33Functions: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          85 : void HdgPassthrough::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      15             : 
+      16          85 :   ch_ = ch;
+      17          85 :   ph_ = ph;
+      18             : 
+      19          85 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      20             : 
+      21          85 :   hdg_state_      = states_t::Zero();
+      22          85 :   hdg_covariance_ = covariance_t::Zero();
+      23             : 
+      24          85 :   innovation_ << 
+      25          85 :     0, 0;
+      26             : 
+      27             : 
+      28             :   // | --------------- param loader initialization --------------- |
+      29             : 
+      30          85 :   if (is_core_plugin_) {
+      31             : 
+      32          85 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      33          85 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + parent_state_est_name_ + "/" + getName() + ".yaml");
+      34             :   }
+      35             : 
+      36          85 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getNamespacedName() + "/");
+      37             : 
+      38             :   // | --------------------- load parameters -------------------- |
+      39          85 :   ph->param_loader->loadParam("max_flight_z", max_flight_z_);
+      40             : 
+      41          85 :   ph->param_loader->loadParam("topics/orientation", orient_topic_);
+      42          85 :   ph->param_loader->loadParam("topics/angular_velocity", ang_vel_topic_);
+      43             : 
+      44          85 :   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          85 :   timer_update_       = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgPassthrough::timerUpdate, this, false, false);  // not running after init
+      51          85 :   timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &HdgPassthrough::timerCheckHealth, this);
+      52             : 
+      53             :   // | --------------- subscribers initialization --------------- |
+      54             :   // subscriber to odometry
+      55         170 :   mrs_lib::SubscribeHandlerOptions shopts;
+      56          85 :   shopts.nh                 = nh;
+      57          85 :   shopts.node_name          = getPrintName();
+      58          85 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      59          85 :   shopts.threadsafe         = true;
+      60          85 :   shopts.autostart          = true;
+      61          85 :   shopts.queue_size         = 10;
+      62          85 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      63             : 
+      64         170 :   sh_orientation_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, "/" + ch_->uav_name + "/" + orient_topic_,
+      65          85 :                                                                                 &HdgPassthrough::callbackOrientation, this);
+      66         170 :   sh_ang_vel_     = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, "/" + ch_->uav_name + "/" + ang_vel_topic_,
+      67          85 :                                                                          &HdgPassthrough::callbackAngularVelocity, this);
+      68             : 
+      69             :   // | ---------------- publishers initialization --------------- |
+      70          85 :   if (ch_->debug_topics.output) {
+      71          85 :     ph_output_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorOutput>(nh, getNamespacedName() + "/output", 10);
+      72             :   }
+      73          85 :   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          85 :   if (changeState(INITIALIZED_STATE)) {
+      80          85 :     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          85 : }
+      85             : /*//}*/
+      86             : 
+      87             : /*//{ start() */
+      88         147 : bool HdgPassthrough::start(void) {
+      89             : 
+      90         147 :   if (isInState(READY_STATE)) {
+      91          85 :     timer_update_.start();
+      92          85 :     changeState(STARTED_STATE);
+      93          85 :     return true;
+      94             : 
+      95             :   } else {
+      96          62 :     ROS_WARN_THROTTLE(1.0, "[%s]: Estimator must be in READY_STATE to start it", getPrintName().c_str());
+      97          62 :     return false;
+      98             :   }
+      99             : }
+     100             : /*//}*/
+     101             : 
+     102             : /*//{ pause() */
+     103           0 : bool HdgPassthrough::pause(void) {
+     104             : 
+     105           0 :   if (isInState(RUNNING_STATE)) {
+     106           0 :     changeState(STOPPED_STATE);
+     107           0 :     return true;
+     108             : 
+     109             :   } else {
+     110           0 :     return false;
+     111             :   }
+     112             : }
+     113             : /*//}*/
+     114             : 
+     115             : /*//{ reset() */
+     116           0 : bool HdgPassthrough::reset(void) {
+     117             : 
+     118           0 :   if (!isInitialized()) {
+     119           0 :     ROS_ERROR("[%s]: Cannot reset uninitialized estimator", getPrintName().c_str());
+     120           0 :     return false;
+     121             :   }
+     122             : 
+     123           0 :   changeState(STOPPED_STATE);
+     124             : 
+     125           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     126             : 
+     127           0 :   return true;
+     128             : }
+     129             : /*//}*/
+     130             : 
+     131             : /*//{ callbackOrientation() */
+     132      177673 : void HdgPassthrough::callbackOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg) {
+     133             : 
+     134      177673 :   if (!isInitialized()) {
+     135           4 :     return;
+     136             :   }
+     137             : 
+     138             :   double hdg;
+     139             :   try {
+     140      177646 :     hdg = mrs_lib::AttitudeConverter(msg->quaternion).getHeading();
+     141             :   }
+     142           0 :   catch (...) {
+     143           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed getting heading", getPrintName().c_str());
+     144             :   }
+     145             : 
+     146      177656 :   setState(hdg, POSITION);
+     147             : 
+     148      177627 :   if (!isError()) {
+     149      177534 :     mrs_lib::set_mutexed(mutex_last_valid_hdg_, hdg, last_valid_hdg_);
+     150             :   }
+     151             : }
+     152             : /*//}*/
+     153             : 
+     154             : /*//{ callbackAngularVelocity() */
+     155      173513 : void HdgPassthrough::callbackAngularVelocity(const geometry_msgs::Vector3Stamped::ConstPtr msg) {
+     156             : 
+     157      173513 :   if (!isInitialized() || !sh_orientation_.hasMsg()) {
+     158           1 :     return;
+     159             :   }
+     160             : 
+     161             :   double hdg_rate;
+     162             :   try {
+     163      173500 :     hdg_rate = mrs_lib::AttitudeConverter(sh_orientation_.getMsg()->quaternion).getHeadingRate(msg->vector);
+     164             :   }
+     165           0 :   catch (...) {
+     166           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: failed getting heading", getPrintName().c_str());
+     167             :   }
+     168             : 
+     169      173339 :   setState(hdg_rate, VELOCITY);
+     170             : }
+     171             : /*//}*/
+     172             : 
+     173             : /* timerUpdate() //{*/
+     174      173563 : void HdgPassthrough::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) {
+     175             : 
+     176             : 
+     177      173563 :   if (!isInitialized()) {
+     178           0 :     return;
+     179             :   }
+     180             : 
+     181      173566 :   publishOutput();
+     182      173573 :   publishDiagnostics();
+     183             : }
+     184             : /*//}*/
+     185             : 
+     186             : /*//{ timerCheckHealth() */
+     187      176503 : void HdgPassthrough::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     188             : 
+     189      176503 :   if (!isInitialized()) {
+     190          13 :     return;
+     191             :   }
+     192             : 
+     193      176502 :   if (isInState(INITIALIZED_STATE) && is_orient_ready_ && is_ang_vel_ready_) {
+     194             : 
+     195          85 :     changeState(READY_STATE);
+     196          85 :     ROS_INFO_THROTTLE(1.0, "[%s]: Ready to start", getPrintName().c_str());
+     197             :   }
+     198             : 
+     199      176502 :   if (isInState(STARTED_STATE)) {
+     200             : 
+     201          85 :     ROS_INFO_THROTTLE(1.0, "[%s]: Estimator Running", getPrintName().c_str());
+     202          85 :     changeState(RUNNING_STATE);
+     203             :   }
+     204             : 
+     205      176515 :   if (sh_orientation_.hasMsg()) {
+     206      175027 :     is_orient_ready_ = true;
+     207             :   } else {
+     208        1473 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received orientation yet", getPrintName().c_str());
+     209             :   }
+     210             : 
+     211      176527 :   if (sh_ang_vel_.hasMsg()) {
+     212      173523 :     is_ang_vel_ready_ = true;
+     213             :   } else {
+     214        2733 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received angular velocity yet", getPrintName().c_str());
+     215             :   }
+     216             : }
+     217             : /*//}*/
+     218             : 
+     219             : /*//{ getState() */
+     220           0 : double HdgPassthrough::getState(const int &state_id_in, const int &axis_in) const {
+     221           0 :   return getState(stateIdToIndex(state_id_in, axis_in));
+     222             : }
+     223             : 
+     224      602773 : double HdgPassthrough::getState(const int &state_idx_in) const {
+     225      602773 :   return mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_(state_idx_in));
+     226             : }
+     227             : /*//}*/
+     228             : 
+     229             : /*//{ setState() */
+     230           0 : void HdgPassthrough::setState(const double &state_in, const int &state_id_in, const int &axis_in) {
+     231           0 :   setState(state_in, stateIdToIndex(state_id_in, axis_in));
+     232           0 : }
+     233             : 
+     234      351175 : void HdgPassthrough::setState(const double &state_in, const int &state_idx_in) {
+     235             : 
+     236      351175 :   const double prev_hdg_state   = mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_(state_idx_in));
+     237      351014 :   prev_hdg_state_(state_idx_in) = prev_hdg_state;
+     238      350949 :   mrs_lib::set_mutexed(mtx_hdg_state_, state_in, hdg_state_(state_idx_in));
+     239             : 
+     240      351002 :   const double innovation = mrs_lib::geometry::radians::dist(mrs_lib::geometry::radians(state_in), mrs_lib::geometry::radians(prev_hdg_state));
+     241      350720 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_(state_idx_in));
+     242      350986 : }
+     243             : /*//}*/
+     244             : 
+     245             : /*//{ getStates() */
+     246      173559 : HdgPassthrough::states_t HdgPassthrough::getStates(void) const {
+     247      173559 :   return mrs_lib::get_mutexed(mtx_hdg_state_, hdg_state_);
+     248             : }
+     249             : /*//}*/
+     250             : 
+     251             : /*//{ setStates() */
+     252           0 : void HdgPassthrough::setStates(const states_t &states_in) {
+     253           0 :   mrs_lib::set_mutexed(mtx_hdg_state_, states_in, hdg_state_);
+     254           0 : }
+     255             : /*//}*/
+     256             : 
+     257             : /*//{ getCovariance() */
+     258           0 : double HdgPassthrough::getCovariance(const int &state_id_in, const int &axis_in) const {
+     259           0 :   return getCovariance(stateIdToIndex(state_id_in, axis_in));
+     260             : }
+     261             : 
+     262           0 : double HdgPassthrough::getCovariance(const int &state_idx_in) const {
+     263           0 :   return mrs_lib::get_mutexed(mtx_hdg_covariance_, hdg_covariance_(state_idx_in, state_idx_in));
+     264             : }
+     265             : /*//}*/
+     266             : 
+     267             : /*//{ getCovarianceMatrix() */
+     268      173530 : HdgPassthrough::covariance_t HdgPassthrough::getCovarianceMatrix(void) const {
+     269      173530 :   return mrs_lib::get_mutexed(mtx_hdg_covariance_, hdg_covariance_);
+     270             : }
+     271             : /*//}*/
+     272             : 
+     273             : /*//{ setCovarianceMatrix() */
+     274           0 : void HdgPassthrough::setCovarianceMatrix(const covariance_t &cov_in) {
+     275           0 :   mrs_lib::set_mutexed(mtx_hdg_covariance_, cov_in, hdg_covariance_);
+     276           0 : }
+     277             : /*//}*/
+     278             : 
+     279             : /*//{ getInnovation() */
+     280           0 : double HdgPassthrough::getInnovation(const int &state_idx) const {
+     281           0 :   return mrs_lib::get_mutexed(mtx_innovation_, innovation_(state_idx));
+     282             : }
+     283             : 
+     284           0 : double HdgPassthrough::getInnovation(const int &state_id_in, const int &axis_in) const {
+     285           0 :   return getInnovation(stateIdToIndex(state_id_in, axis_in));
+     286             : }
+     287             : /*//}*/
+     288             : 
+     289             : /*//{ getLastValidHdg() */
+     290           0 : double HdgPassthrough::getLastValidHdg() const {
+     291           0 :   return mrs_lib::get_mutexed(mutex_last_valid_hdg_, last_valid_hdg_);
+     292             : }
+     293             : /*//}*/
+     294             : 
+     295             : /*//{ getNamespacedName() */
+     296         170 : std::string HdgPassthrough::getNamespacedName() const {
+     297         340 :   return parent_state_est_name_ + "/" + getName();
+     298             : }
+     299             : /*//}*/
+     300             : 
+     301             : /*//{ getPrintName() */
+     302         503 : std::string HdgPassthrough::getPrintName() const {
+     303        1006 :   return ch_->nodelet_name + "/" + parent_state_est_name_ + "/" + getName();
+     304             : }
+     305             : /*//}*/
+     306             : 
+     307             : };  // namespace mrs_uav_state_estimators
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html new file mode 100644 index 0000000000..519a26d886 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp.gcov.overview.html @@ -0,0 +1,97 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/heading/hdg_passthrough.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

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

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

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

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

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

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

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

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

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

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

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/lateral/lat_generic.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..508d03d81b4e53c31168d589e7b140e4bb5e1fec GIT binary patch literal 3146 zcmV-Q47Kx#P)_F@S;5{wS;5}G*?*N6>jy}HP|j}LK3%VuODPrz5I-ryMTpSeE>6cv(Kgju9By;G-wTKn?T=mzEA279A;Z^cfaW;{*|*Kn zgwguw4-~uOcUl;e9UZg5n!vhF-NtDK%dwopo*C(aFubC70Ggz+XJv#ZA12P>H@a*O z+Tq5ymB3qhWhA2Exi0ci1p}5CFa~fDu-sJ$;KFP<38{>cqMWgq`Jak>T-TR8w)A3O zhjm{~qeB-8<;qHBTS6cZse(~@=3}w^dLj8^m}JozKm4Py1ca?Mu3H6IFBy+% zOb=|*FdqXW8zy?ll4Aj6M|h3|=l}%2u@u&<1MoLIn!y?0>HzE(3L+I?Ai?=KKum%w znlrrXESQ*LJ6fK^oSgtzfU+yY3vNFjlX1dqrTa{Xxt65I0!9J|Ixhes8YN|n5t4xz zGu1P*BnvYEyQ^S+H1AVzrr9_=#`i|>e)-zA=#eOZevjBTWrK)w^sfPs8GUnnKX<3U zU(fSePl`k*k~2)VDMlTMP`sB^bV@&(qwehu$=vz0m}bqmnG33bR8LN;0LBOyxupP> z+!O&_#3($wesmMiS&8vT>lJqy&^XYeHTeM>?pkrAvLVH>B#hQMA~Bm(G3zjU29o`z zX$_l;M~iGq(T5TehW+K!N6QNYHoP=wm3{o4H8YPo`rs*taL?s$K?WBZ-4BO8srj3l zkMY|P&uloN>E)wwspsKoNx9BFX`JMQHUX*UD>JGqM`{z?nLx@3%QCcAF?xF1b2N^3 zAZ~WpF&jZF7Z-_KLLMW}v2Ag!mcp!Lja~YKVHPv0LYu5JHq|-+3C6ealQA~jJOa42 zvJ^dYG)qrgO8pDeS)2B7LITA2Ya(^almH};H^JRjY z9H|#^Oh01J%Ve3?!AQ$`h1r8#l@3_d6BhI@^1wcFxztE5I3K27g-Z&HICZJa(fH^? z!a=fSdw+--(P{AOiDd2(&?1jUrRiYLoY)VXE!{KB2CY%YgMD8433}V2mr`Ul-Zs%A`w; zHqY@GCa08rbSjl1-ey2c2FWC7eu_phtC`~}MQ~N}N(&(GpffES&aX>~F!9R3*3!UO zp>(x8R)9+C)5xD+!*ILpF(eFw--CCa=ocRw(~kV)3Q%&JJ)mVv>QycS!_QwCw6V9M zxN2j6<=q`Qz2Dr}ff$t*g_xZ>(LF)M8l?*&Gj0wZkj4o7T>Ya7*v5y(72wMTxRYR| z1T-xT<)fDc8vpDF&@}98J%lJAci2oZ6H;m1N`2QbMTwZ4o=9{s@n+zoAtUy=?eQ`s zVLaFNm`6!8IQEoBh&?;X@8?FPJ;?>b$3S_>bHW}-8+q(oX?v&Ec zM%mMTa)%anEulfmx^gL}8~fAS@ya@vF*LVgvPZo zsE(3k^yx4p(^R}k5BuU*Z<$k`XL-UGDBUfVFw?Rm75CF~u?a6N;Wy?upHTrrRiY}0sBKRdAZ<=Q5$-S2l zk2AJD8+%N(2w2!Xo}YYI6VGsCkUJOMoNC9A5zsbbV$TclIK_GwY0QZQFuD!9XS69f z^_u%&iN)0>Ng1wsDQH7iiRoat-U1AOSC|wyUap6GJYYS>;+21N{o6cyY;g7wWqN!R0d~2~#%{dIj+E|KH1Pg?xVJh5LCpN+vB1m-ogvImFtqorGWQM!?qzp27Q5OlK`=lnD zKhrSqFLi}nAEABCcOXWm2NJ%J#sxY`Kzri+u-}Z1)qq=j^GbPKz$bKcv6_at2Vy` z_J6r7jT-3V%@6sA>;w@)#T~(#*7i|Q*st2h(~F9XLnte@s7 zS{aWF_#;%Ox;`|gy1q*o?|-ntR{_RQc0r|QZ?E& z)~2VBQ>U-_D;Hl=u^9XsUsA@#T;WCRaP(hgXsNLuVQpnNVa0!ma6KvPDuCPtxJSQ~J}^U46r_rp_< z-g>-jz<Kt#w9+p4WwJxY$eW7UeBAxgWS;YeCRHx}F?ll1BV30fc*$v3ftOWdw z;abe4Fg)S7V2;PI`dqzdd=~X$hYQMp=FwAaw)$ollzhn>(&dEtCY!Kl9)McQU;;c^ zzEnW;GwB-Wu8VYS^=mW6wTQ#_WH2_+ny53M)~}-%!W=A3c!$m{O6Se~qG!QzmwOEGnPcb5 z>#u4S4}{FMQDfes!_MneQTDBZz<6G|?@JI^FCF zE_o?)W3XT!Yc>1<`-tc0Q1Yo^ACo%XLmr!p!xK>}`K+Xi};Ja1MD^C`|h zq=m%jN?J&-yXabL^U$>LVoPzAJoaCY8O|tjbL{xIC_l~zY^4AKpk_-k(5HSBUyH+QZY#7%Ap%{bfU(=)gO zU=NL)h>u40tT=&;FoyikMgWh8n=r0#_ zxEMel7XuiVY~xTD7&F^>z|2Sa$xw^4bk7>teTMX8@eO^Tdz794(Q3!DOOHi-Qc1(a zo-YGtO>SaL5~c#0q{lqs48J92IphwFR5GCF;X(5zPZa@rD`7qN!H-y~G&axOAo?)m zdlt*S-hc4R@NuIZA3+&zL2&8AyZGNrfQvtBA3ix%2NY4&fC4H8V{|T`?5*|$Blw&g zU<7D=nCBh*oQ{-GA#cVeAEHV^jPv^6K85JuBa{xQVi7#y$Kq# k-~y^2L%vD~mXd+{4}ZzB4 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - gps_baro.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()16
mrs_uav_state_estimators::gps_baro::GpsBaro::GpsBaro()16
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro()16
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro().216
+
+
+ + + +
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..d4025dad52 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.func.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - gps_baro.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()16
mrs_uav_state_estimators::gps_baro::GpsBaro::GpsBaro()16
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro()16
mrs_uav_state_estimators::gps_baro::GpsBaro::~GpsBaro().216
+
+
+ + + +
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..21426a2929 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.html @@ -0,0 +1,109 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - gps_baro.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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          16 :   GpsBaro() : StateGeneric(estimator_name, is_core_plugin) {
+      15          16 :   }
+      16             : 
+      17          32 :   ~GpsBaro(void) {
+      18          32 :   }
+      19             : };
+      20             : 
+      21             : }  // namespace gps_baro
+      22             : }  // namespace mrs_uav_state_estimators
+      23             : 
+      24             : #include <pluginlib/class_list_macros.h>
+      25          16 : PLUGINLIB_EXPORT_CLASS(mrs_uav_state_estimators::gps_baro::GpsBaro, mrs_uav_managers::StateEstimator)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.overview.html new file mode 100644 index 0000000000..2b03dc176f --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp.gcov.overview.html @@ -0,0 +1,27 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_baro.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()65
mrs_uav_state_estimators::gps_garmin::GpsGarmin::GpsGarmin()65
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin()65
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin().265
+
+
+ + + +
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..7ddb03b413 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.func.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - gps_garmin.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions:44100.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()65
mrs_uav_state_estimators::gps_garmin::GpsGarmin::GpsGarmin()65
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin()65
mrs_uav_state_estimators::gps_garmin::GpsGarmin::~GpsGarmin().265
+
+
+ + + +
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..c6e9f3d3e4 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp.gcov.html @@ -0,0 +1,109 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/gps_garmin.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - gps_garmin.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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          65 :   GpsGarmin() : StateGeneric(estimator_name, is_core_plugin) {
+      15          65 :   }
+      16             : 
+      17         130 :   ~GpsGarmin(void) {
+      18         130 :   }
+      19             : };
+      20             : 
+      21             : }  // namespace gps_garmin
+      22             : }  // namespace mrs_uav_state_estimators
+      23             : 
+      24             : #include <pluginlib/class_list_macros.h>
+      25          65 : 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..70fa8bdf0a --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-detail-sort-f.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:31144070.7 %
Date:2024-04-09 22:03:33Functions:283775.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
<unnamed>76.4 %97 / 12755.6 %5 / 9
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
<unnamed>66.2 %194 / 29358.3 %7 / 12
rtk_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
rtk.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..b19df9df7a --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-detail-sort-l.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:31144070.7 %
Date:2024-04-09 22:03:33Functions:283775.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
<unnamed>66.2 %194 / 29358.3 %7 / 12
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
<unnamed>76.4 %97 / 12755.6 %5 / 9
rtk_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
<unnamed>100.0 %5 / 5100.0 %4 / 4
rtk.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..6141925793 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-detail.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:31144070.7 %
Date:2024-04-09 22:03:33Functions:283775.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
rtk_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
rtk.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..54330d225b --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index-sort-l.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:31144070.7 %
Date:2024-04-09 22:03:33Functions:283775.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
state_generic.cpp +
66.2%66.2%
+ 		66.2 %194 / 29358.3 %7 / 12
passthrough.cpp +
76.4%76.4%
+ 		76.4 %97 / 12755.6 %5 / 9
rtk_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
gps_garmin.cpp +
100.0%
+ 		100.0 %5 / 5100.0 %4 / 4
rtk.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..1b58f95fb3 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/index.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/stateHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:31144070.7 %
Date:2024-04-09 22:03:33Functions:283775.7 %
Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

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

+ Overview +
+ + diff --git a/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.png b/mrs_uav_state_estimators/src/estimators/state/passthrough.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..a57b3d1729bc3cdac5e4ae2ad4e24468373c0602 GIT binary patch literal 1099 zcmV-R1ho5!P)CR&-|8ai1Q<*+6d4na1&0Mk zrYzb^4VF?m$^_?ucXr{IX=DbBSx|OS#%OII(-i59jDZGlK*u>+0!@d@)G^qPw`1vA z5I8zu8oC^q8cqViaU^8tqMJbA8e^E|!GRmdB+OBks`;Z>FYQ>7@56w1Mg`no%WEq{K{?J={g%RO<`JEIPElp`a-e>NZIyFAu`fr1cTx;zevpp>H-w=Hez0H1or28Hphso^eiqdNx~U5^`VZgEm@}w4@WW}Lj9fl` zQh^>fSSI5s)`=AI-C#5(vSv*jj^M8h#8fwzUQBX)Ehq= z&a44HYI+|ysN>BqtwJfV;+~R5PyGnKID!Os`AF$kci}8L-2Nn&UjS;DQ8lVZUjRF- z&d7zg1vD^YxKZLm-~(f9TvTjOp5YdgLT@GLD!yc0P002ovPDHLkV1lbW1y=w7 literal 0 HcmV?d00001 diff --git a/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func-sort-c.html b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func-sort-c.html new file mode 100644 index 0000000000..95c000d18a --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func-sort-c.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - rtk.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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..3710ec7dae --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.func.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - rtk.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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..8b1a043c51 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk.cpp.gcov.html @@ -0,0 +1,109 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - rtk.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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..2fa6b22e94 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.func-sort-c.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - rtk_garmin.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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..ef00f3e7dd --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.func.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - rtk_garmin.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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..0704988109 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp.gcov.html @@ -0,0 +1,109 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/rtk_garmin.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - rtk_garmin.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:55100.0 %
Date:2024-04-09 22:03:33Functions: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..e580d9c40b --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func-sort-c.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/state_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - state_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19429366.2 %
Date:2024-04-09 22:03:33Functions: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&)85
mrs_uav_state_estimators::StateGeneric::start()6846
mrs_uav_state_estimators::StateGeneric::updateUavState()166846
mrs_uav_state_estimators::StateGeneric::timerPubAttitude(ros::TimerEvent const&)176622
mrs_uav_state_estimators::StateGeneric::timerUpdate(ros::TimerEvent const&)176630
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()() const262534
mrs_uav_state_estimators::StateGeneric::getHeading() const262541
+
+
+ + + +
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..002e0d0700 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.func.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/state_generic.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - state_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19429366.2 %
Date:2024-04-09 22:03:33Functions: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&)85
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&)176630
mrs_uav_state_estimators::StateGeneric::updateUavState()166846
mrs_uav_state_estimators::StateGeneric::timerCheckHealth(ros::TimerEvent const&)0
mrs_uav_state_estimators::StateGeneric::timerPubAttitude(ros::TimerEvent const&)176622
mrs_uav_state_estimators::StateGeneric::pause()0
mrs_uav_state_estimators::StateGeneric::reset()0
mrs_uav_state_estimators::StateGeneric::start()6846
mrs_uav_state_estimators::StateGeneric::getHeading() const262541
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()() const262534
+
+
+ + + +
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..6b6961d9a1 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.html @@ -0,0 +1,633 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/state_generic.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_state_estimators/src/estimators/state - state_generic.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:19429366.2 %
Date:2024-04-09 22:03:33Functions: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          85 : void StateGeneric::initialize(ros::NodeHandle &parent_nh, const std::shared_ptr<CommonHandlers_t> &ch, const std::shared_ptr<PrivateHandlers_t> &ph) {
+      12             : 
+      13          85 :   ch_ = ch;
+      14          85 :   ph_ = ph;
+      15             : 
+      16         170 :   ros::NodeHandle nh(parent_nh);
+      17             : 
+      18          85 :   if (is_core_plugin_) {
+      19             : 
+      20          85 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml");
+      21          85 :     ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml");
+      22             :   }
+      23             : 
+      24          85 :   ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/");
+      25             : 
+      26          85 :   ph->param_loader->loadParam("estimators/lateral/name", est_lat_name_);
+      27          85 :   ph->param_loader->loadParam("estimators/altitude/name", est_alt_name_);
+      28          85 :   ph->param_loader->loadParam("estimators/heading/name", est_hdg_name_);
+      29          85 :   ph->param_loader->loadParam("estimators/heading/passthrough", is_hdg_passthrough_);
+      30             : 
+      31          85 :   ph->param_loader->loadParam("override_frame_id/enabled", is_override_frame_id_);
+      32          85 :   if (is_override_frame_id_) {
+      33           0 :     ph->param_loader->loadParam("override_frame_id/frame_id", frame_id_);
+      34             :   }
+      35             : 
+      36         170 :   std::string topic_orientation;
+      37          85 :   ph->param_loader->loadParam("topics/orientation", topic_orientation);
+      38          85 :   topic_orientation_ = "/" + ch_->uav_name + "/" + topic_orientation;
+      39         170 :   std::string topic_angular_velocity;
+      40          85 :   ph->param_loader->loadParam("topics/angular_velocity", topic_angular_velocity);
+      41          85 :   topic_angular_velocity_ = "/" + ch_->uav_name + "/" + topic_angular_velocity;
+      42             : 
+      43          85 :   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          85 :   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
+      49             : 
+      50             :   // | ------------------ timers initialization ----------------- |
+      51          85 :   timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerUpdate, this);  // not running after init
+      52             :   /* timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerCheckHealth, this); */
+      53          85 :   timer_pub_attitude_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerPubAttitude, this);
+      54             : 
+      55             :   // | --------------- subscribers initialization --------------- |
+      56         170 :   mrs_lib::SubscribeHandlerOptions shopts;
+      57          85 :   shopts.nh                 = nh;
+      58          85 :   shopts.node_name          = getPrintName();
+      59          85 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+      60          85 :   shopts.threadsafe         = true;
+      61          85 :   shopts.autostart          = true;
+      62          85 :   shopts.queue_size         = 10;
+      63          85 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+      64             : 
+      65          85 :   sh_hw_api_orient_  = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, topic_orientation_);
+      66          85 :   sh_hw_api_ang_vel_ = mrs_lib::SubscribeHandler<geometry_msgs::Vector3Stamped>(shopts, topic_angular_velocity_);
+      67             : 
+      68             :   // | ---------------- publishers initialization --------------- |
+      69          85 :   ph_odom_     = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/odom", 10);
+      70          85 :   ph_attitude_ = mrs_lib::PublisherHandler<geometry_msgs::QuaternionStamped>(nh, Support::toSnakeCase(getName()) + "/attitude", 10);
+      71             : 
+      72          85 :   if (ch_->debug_topics.state) {
+      73          85 :     ph_uav_state_ = mrs_lib::PublisherHandler<mrs_msgs::UavState>(nh, Support::toSnakeCase(getName()) + "/uav_state", 10);
+      74             :   }
+      75          85 :   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          85 :   if (ch_->debug_topics.innovation) {
+      80           0 :     ph_innovation_ = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh, Support::toSnakeCase(getName()) + "/innovation", 10);
+      81             :   }
+      82          85 :   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         170 :   std::vector<double> max_altitudes;
+      88             : 
+      89          85 :   if (is_hdg_passthrough_) {
+      90          85 :     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          85 :   est_hdg_->initialize(nh, ch_, ph_);
+      95          85 :   max_altitudes.push_back(est_hdg_->getMaxFlightZ());
+      96             : 
+      97      262619 :   est_lat_ = std::make_unique<LatGeneric>(est_lat_name_, frame_id_, getName(), is_core_plugin_, [this](void) { return this->getHeading(); });
+      98          85 :   est_lat_->initialize(nh, ch_, ph_);
+      99          85 :   max_altitudes.push_back(est_lat_->getMaxFlightZ());
+     100             : 
+     101          85 :   est_alt_ = std::make_unique<AltGeneric>(est_alt_name_, frame_id_, getName(), is_core_plugin_);
+     102          85 :   est_alt_->initialize(nh, ch_, ph_);
+     103          85 :   max_altitudes.push_back(est_alt_->getMaxFlightZ());
+     104             : 
+     105          85 :   max_flight_z_ = *std::min_element(max_altitudes.begin(), max_altitudes.end());
+     106             : 
+     107             :   // | ------------------ initialize published messages ------------------ |
+     108          85 :   uav_state_init_.header.frame_id = ns_frame_id_;
+     109          85 :   uav_state_init_.child_frame_id  = ch_->frames.ns_fcu;
+     110             : 
+     111          85 :   uav_state_init_.estimator_horizontal = est_lat_name_;
+     112          85 :   uav_state_init_.estimator_vertical   = est_alt_name_;
+     113          85 :   uav_state_init_.estimator_heading    = est_hdg_name_;
+     114             : 
+     115          85 :   innovation_init_.header.frame_id         = ns_frame_id_;
+     116          85 :   innovation_init_.child_frame_id          = ch_->frames.ns_fcu;
+     117          85 :   innovation_init_.pose.pose.orientation.w = 1.0;
+     118             : 
+     119             :   // | ------------------ finish initialization ----------------- |
+     120             : 
+     121          85 :   if (changeState(INITIALIZED_STATE)) {
+     122          85 :     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          85 : }
+     127             : /*//}*/
+     128             : 
+     129             : /*//{ start() */
+     130        6846 : bool StateGeneric::start(void) {
+     131             : 
+     132        6846 :   if (isInState(READY_STATE)) {
+     133             : 
+     134             :     bool est_lat_start_successful, est_alt_start_successful, est_hdg_start_successful;
+     135             : 
+     136        6846 :     if (est_lat_->isStarted() || est_lat_->isRunning()) {
+     137        2886 :       est_lat_start_successful = true;
+     138             :     } else {
+     139        3960 :       est_lat_start_successful = est_lat_->start();
+     140             :     }
+     141             : 
+     142        6846 :     if (est_alt_->isStarted() || est_alt_->isRunning()) {
+     143        3301 :       est_alt_start_successful = true;
+     144             :     } else {
+     145        3545 :       est_alt_start_successful = est_alt_->start();
+     146             :     }
+     147             : 
+     148        6846 :     if (est_hdg_->isStarted() || est_hdg_->isRunning()) {
+     149        6699 :       timer_pub_attitude_.start();
+     150        6699 :       est_hdg_start_successful = true;
+     151             :     } else {
+     152         147 :       est_hdg_start_successful = est_hdg_->start();
+     153             :     }
+     154             : 
+     155        6846 :     if (est_lat_start_successful && est_alt_start_successful && est_hdg_start_successful) {
+     156             :       /* timer_update_.start(); */
+     157          85 :       changeState(STARTED_STATE);
+     158          85 :       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        6761 :   return false;
+     166             : 
+     167             :   ROS_ERROR("[%s]: Failed to start", getPrintName().c_str());
+     168             :   return false;
+     169             : }
+     170             : /*//}*/
+     171             : 
+     172             : /*//{ pause() */
+     173           0 : bool StateGeneric::pause(void) {
+     174             : 
+     175           0 :   if (isInState(RUNNING_STATE)) {
+     176           0 :     est_lat_->pause();
+     177           0 :     est_alt_->pause();
+     178           0 :     est_hdg_->pause();
+     179           0 :     changeState(STOPPED_STATE);
+     180           0 :     return true;
+     181             : 
+     182             :   } else {
+     183           0 :     return false;
+     184             :   }
+     185             : }
+     186             : /*//}*/
+     187             : 
+     188             : /*//{ reset() */
+     189           0 : bool StateGeneric::reset(void) {
+     190             : 
+     191           0 :   if (!isInitialized()) {
+     192           0 :     ROS_ERROR("[%s]: Cannot reset uninitialized estimator", getPrintName().c_str());
+     193           0 :     return false;
+     194             :   }
+     195             : 
+     196           0 :   est_lat_->pause();
+     197           0 :   est_alt_->pause();
+     198           0 :   est_hdg_->pause();
+     199           0 :   changeState(STOPPED_STATE);
+     200             : 
+     201           0 :   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
+     202             : 
+     203           0 :   return true;
+     204             : }
+     205             : /*//}*/
+     206             : 
+     207             : /* timerUpdate() //{*/
+     208      176630 : void StateGeneric::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) {
+     209             : 
+     210             : 
+     211      176630 :   if (!isInitialized()) {
+     212        9761 :     return;
+     213             :   }
+     214             : 
+     215      352099 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerUpdate", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     216             : 
+     217      176039 :   switch (getCurrentSmState()) {
+     218             : 
+     219           0 :     case UNINITIALIZED_STATE: {
+     220           0 :       break;
+     221             :     }
+     222        2369 :     case INITIALIZED_STATE: {
+     223             : 
+     224        2369 :       if (sh_hw_api_orient_.hasMsg() && sh_hw_api_ang_vel_.hasMsg()) {
+     225          85 :         if (est_lat_->isInitialized() && est_alt_->isInitialized() && est_hdg_->isInitialized()) {
+     226          85 :           changeState(READY_STATE);
+     227          85 :           ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is ready to start", getPrintName().c_str());
+     228             :         } else {
+     229           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s subestimators to be initialized", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     230           0 :           return;
+     231             :         }
+     232             :       } else {
+     233        2284 :         ROS_INFO_THROTTLE(1.0, "[%s]: %s msg on topic %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), sh_hw_api_orient_.topicName().c_str());
+     234        2284 :         return;
+     235             :       }
+     236             : 
+     237          85 :       break;
+     238             :     }
+     239             : 
+     240        6799 :     case READY_STATE: {
+     241        6799 :       break;
+     242             :     }
+     243             : 
+     244         104 :     case STARTED_STATE: {
+     245             : 
+     246         104 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     247             : 
+     248         104 :       if (est_lat_->isRunning() && est_alt_->isRunning() && est_hdg_->isRunning()) {
+     249          85 :         ROS_INFO_THROTTLE(1.0, "[%s]: Subestimators converged", getPrintName().c_str());
+     250          85 :         changeState(RUNNING_STATE);
+     251             :       } else {
+     252          19 :         return;
+     253             :       }
+     254          85 :       break;
+     255             :     }
+     256             : 
+     257      166778 :     case RUNNING_STATE: {
+     258      166778 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     259           0 :         changeState(ERROR_STATE);
+     260             :       }
+     261      166753 :       break;
+     262             :     }
+     263             : 
+     264           0 :     case STOPPED_STATE: {
+     265           0 :       break;
+     266             :     }
+     267             : 
+     268           0 :     case ERROR_STATE: {
+     269           0 :       if ((est_lat_->isReady() || est_lat_->isRunning()) && (est_alt_->isReady() || est_alt_->isRunning()) && (est_hdg_->isReady() || est_hdg_->isRunning())) {
+     270           0 :         changeState(READY_STATE);
+     271             :       }
+     272           0 :       break;
+     273             :     }
+     274             :   }
+     275             : 
+     276      173713 :   if (!isRunning() && !isStarted()) {
+     277        6884 :     return;
+     278             :   }
+     279             : 
+     280      166846 :   updateUavState();
+     281             : 
+     282      166821 :   publishUavState();
+     283      166870 :   publishOdom();
+     284      166872 :   publishCovariance();
+     285      166872 :   publishInnovation();
+     286      166871 :   publishDiagnostics();
+     287             : }
+     288             : /*//}*/
+     289             : 
+     290             : /*//{ timerCheckHealth() */
+     291           0 : void StateGeneric::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) {
+     292             : 
+     293           0 :   if (!isInitialized()) {
+     294           0 :     return;
+     295             :   }
+     296             : 
+     297           0 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerCheckHealth", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     298             : 
+     299           0 :   switch (getCurrentSmState()) {
+     300             : 
+     301           0 :     case UNINITIALIZED_STATE: {
+     302           0 :       break;
+     303             :     }
+     304           0 :     case INITIALIZED_STATE: {
+     305             : 
+     306           0 :       if (sh_hw_api_orient_.hasMsg() && sh_hw_api_ang_vel_.hasMsg()) {
+     307           0 :         if (est_lat_->isInitialized() && est_alt_->isInitialized() && est_hdg_->isInitialized()) {
+     308           0 :           changeState(READY_STATE);
+     309           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is ready to start", getPrintName().c_str());
+     310             :         } else {
+     311           0 :           ROS_INFO_THROTTLE(1.0, "[%s]: %s subestimators to be initialized", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     312           0 :           return;
+     313             :         }
+     314             :       } else {
+     315           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: %s msg on topic %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), sh_hw_api_orient_.topicName().c_str());
+     316           0 :         return;
+     317             :       }
+     318             : 
+     319           0 :       break;
+     320             :     }
+     321             : 
+     322           0 :     case READY_STATE: {
+     323           0 :       break;
+     324             :     }
+     325             : 
+     326           0 :     case STARTED_STATE: {
+     327             : 
+     328           0 :       ROS_INFO_THROTTLE(1.0, "[%s]: %s for convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
+     329             : 
+     330           0 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     331           0 :         changeState(ERROR_STATE);
+     332             :       }
+     333             : 
+     334           0 :       if (est_lat_->isRunning() && est_alt_->isRunning() && est_hdg_->isRunning()) {
+     335           0 :         ROS_INFO_THROTTLE(1.0, "[%s]: Subestimators converged", getPrintName().c_str());
+     336           0 :         changeState(RUNNING_STATE);
+     337             :       } else {
+     338           0 :         return;
+     339             :       }
+     340           0 :       break;
+     341             :     }
+     342             : 
+     343           0 :     case RUNNING_STATE: {
+     344           0 :       if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+     345           0 :         changeState(ERROR_STATE);
+     346             :       }
+     347           0 :       break;
+     348             :     }
+     349             : 
+     350           0 :     case STOPPED_STATE: {
+     351           0 :       break;
+     352             :     }
+     353             : 
+     354           0 :     case ERROR_STATE: {
+     355           0 :       if (est_lat_->isReady() && est_alt_->isReady() && est_hdg_->isReady()) {
+     356           0 :         changeState(READY_STATE);
+     357             :       }
+     358           0 :       break;
+     359             :     }
+     360             :   }
+     361             : }
+     362             : /*//}*/
+     363             : 
+     364             : /* timerPubAttitude() //{*/
+     365      176622 : void StateGeneric::timerPubAttitude([[maybe_unused]] const ros::TimerEvent &event) {
+     366             : 
+     367      176622 :   if (!isInitialized()) {
+     368        3092 :     return;
+     369             :   }
+     370             : 
+     371      352084 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::timerPubAttitude", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     372             : 
+     373      176020 :   if (!sh_hw_api_orient_.hasMsg()) {
+     374        1419 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received orientation on topic %s yet", getPrintName().c_str(), sh_hw_api_orient_.topicName().c_str());
+     375        1419 :     return;
+     376             :   }
+     377             : 
+     378      174616 :   if (!est_hdg_->isRunning() && !isError()) {
+     379        1100 :     ROS_WARN_THROTTLE(1.0, "[%s]: cannot publish attitude, heading estimator is not running", getPrintName().c_str());
+     380        1100 :     return;
+     381             :   }
+     382             : 
+     383      173522 :   scope_timer.checkpoint("checks");
+     384             : 
+     385      173482 :   const ros::Time time_now = ros::Time::now();
+     386             : 
+     387      173539 :   geometry_msgs::QuaternionStamped att;
+     388      173477 :   att.header.stamp    = time_now;
+     389      173477 :   att.header.frame_id = ns_frame_id_ + "_att_only";
+     390             : 
+     391             :   double hdg;
+     392      173500 :   if (isError()) {
+     393           0 :     hdg = est_hdg_->getLastValidHdg();
+     394             :   } else {
+     395      173501 :     hdg = est_hdg_->getState(POSITION);
+     396             :   }
+     397             : 
+     398      173485 :   auto res = rotateQuaternionByHeading(sh_hw_api_orient_.getMsg()->quaternion, hdg);
+     399      173327 :   if (res) {
+     400      173460 :     att.quaternion = res.value();
+     401             :   } else {
+     402           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: could not rotate orientation by heading", getPrintName().c_str());
+     403           0 :     return;
+     404             :   }
+     405             : 
+     406      173229 :   scope_timer.checkpoint("rotate");
+     407             : 
+     408      173428 :   if (!Support::noNans(att.quaternion)) {
+     409           0 :     ROS_ERROR_THROTTLE(1.0, "[%s]: NaNs in timerPubAttitude quaternion", getPrintName().c_str());
+     410           0 :     return;
+     411             :   }
+     412             : 
+     413      173377 :   scope_timer.checkpoint("nan check");
+     414             : 
+     415      173288 :   ph_attitude_.publish(att);
+     416      173540 :   scope_timer.checkpoint("publish");
+     417             : }
+     418             : /*//}*/
+     419             : 
+     420             : /*//{ isConverged() */
+     421           0 : bool StateGeneric::isConverged() {
+     422             : 
+     423             :   // TODO: check convergence by rate of change of determinant
+     424             :   // most likely not used in top-level estimator
+     425             : 
+     426           0 :   return true;
+     427             : }
+     428             : /*//}*/
+     429             : 
+     430             : /*//{ updateUavState() */
+     431      166846 : void StateGeneric::updateUavState() {
+     432             : 
+     433      166846 :   if (!sh_hw_api_orient_.hasMsg()) {
+     434           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received orientation on topic %s yet", getPrintName().c_str(), sh_hw_api_orient_.topicName().c_str());
+     435           0 :     return;
+     436             :   }
+     437             : 
+     438      166831 :   if (!sh_hw_api_ang_vel_.hasMsg()) {
+     439           0 :     ROS_WARN_THROTTLE(1.0, "[%s]: has not received angular velocity on topic %s yet", getPrintName().c_str(), sh_hw_api_ang_vel_.topicName().c_str());
+     440           0 :     return;
+     441             :   }
+     442      333711 :   mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("StateGeneric::updateUavState", ch_->scope_timer.logger, ch_->scope_timer.enabled);
+     443             : 
+     444      166860 :   const ros::Time time_now = ros::Time::now();
+     445             : 
+     446      166871 :   mrs_msgs::UavState uav_state = uav_state_init_;
+     447      166861 :   uav_state.header.stamp       = time_now;
+     448             : 
+     449             :   // do not rotate orientation if passthrough hdg
+     450      166861 :   if (est_hdg_name_ == "hdg_passthrough") {
+     451           0 :     uav_state.pose.orientation = sh_hw_api_orient_.getMsg()->quaternion;
+     452             :   } else {
+     453             :     double hdg;
+     454      166856 :     if (isError()) {
+     455           0 :       hdg = est_hdg_->getLastValidHdg();
+     456             :     } else {
+     457      166843 :       hdg = est_hdg_->getState(POSITION);
+     458             :     }
+     459             : 
+     460      166832 :     auto res = rotateQuaternionByHeading(sh_hw_api_orient_.getMsg()->quaternion, hdg);
+     461      166723 :     if (res) {
+     462      166832 :       uav_state.pose.orientation = res.value();
+     463             :     } else {
+     464           0 :       ROS_ERROR_THROTTLE(1.0, "[%s]: could not rotate orientation by heading", getPrintName().c_str());
+     465           0 :       return;
+     466             :     }
+     467             :   }
+     468             : 
+     469      166711 :   scope_timer.checkpoint("rotate orientation");
+     470             : 
+     471      166825 :   uav_state.velocity.angular = sh_hw_api_ang_vel_.getMsg()->vector;
+     472             : 
+     473      166788 :   uav_state.pose.position.x = est_lat_->getState(POSITION, AXIS_X);
+     474      166804 :   uav_state.pose.position.y = est_lat_->getState(POSITION, AXIS_Y);
+     475      166816 :   uav_state.pose.position.z = est_alt_->getState(POSITION);
+     476             : 
+     477      166738 :   uav_state.velocity.linear.x = est_lat_->getState(VELOCITY, AXIS_X);  // in global frame
+     478      166798 :   uav_state.velocity.linear.y = est_lat_->getState(VELOCITY, AXIS_Y);  // in global frame
+     479      166815 :   uav_state.velocity.linear.z = est_alt_->getState(VELOCITY);          // in global frame
+     480             : 
+     481      166793 :   uav_state.acceleration.linear.x = est_lat_->getState(ACCELERATION, AXIS_X);  // in global frame
+     482      166805 :   uav_state.acceleration.linear.y = est_lat_->getState(ACCELERATION, AXIS_Y);  // in global frame
+     483      166817 :   uav_state.acceleration.linear.z = est_alt_->getState(ACCELERATION);          // in global frame
+     484             : 
+     485      166808 :   scope_timer.checkpoint("fill uav state");
+     486             : 
+     487      333617 :   const nav_msgs::Odometry odom = Support::uavStateToOdom(uav_state);
+     488      166844 :   scope_timer.checkpoint("uav state to odom");
+     489             : 
+     490      333611 :   nav_msgs::Odometry innovation = innovation_init_;
+     491      166855 :   innovation.header.stamp       = time_now;
+     492             : 
+     493      166855 :   innovation.pose.pose.position.x = est_lat_->getInnovation(POSITION, AXIS_X);
+     494      166752 :   innovation.pose.pose.position.y = est_lat_->getInnovation(POSITION, AXIS_Y);
+     495      166774 :   innovation.pose.pose.position.z = est_alt_->getInnovation(POSITION);
+     496             : 
+     497      166730 :   is_mitigating_jump_ = est_alt_->isMitigatingJump() || est_lat_->isMitigatingJump() || est_hdg_->isMitigatingJump();
+     498             : 
+     499      166836 :   scope_timer.checkpoint("innovation");
+     500             : 
+     501      333516 :   mrs_msgs::Float64ArrayStamped pose_covariance, twist_covariance;
+     502      166685 :   pose_covariance.header.stamp  = time_now;
+     503      166685 :   twist_covariance.header.stamp = time_now;
+     504             : 
+     505      166685 :   const int n_states = 6;  // TODO this should be defined somewhere else
+     506      166685 :   pose_covariance.values.resize(n_states * n_states);
+     507      166754 :   pose_covariance.values.at(n_states * AXIS_X + AXIS_X) = est_lat_->getCovariance(POSITION, AXIS_X);
+     508      166605 :   pose_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = est_lat_->getCovariance(POSITION, AXIS_Y);
+     509      166743 :   pose_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = est_alt_->getCovariance(POSITION);
+     510             : 
+     511      166629 :   twist_covariance.values.resize(n_states * n_states);
+     512      166729 :   twist_covariance.values.at(n_states * AXIS_X + AXIS_X) = est_lat_->getCovariance(VELOCITY, AXIS_X);
+     513      166783 :   twist_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = est_lat_->getCovariance(VELOCITY, AXIS_Y);
+     514      166759 :   twist_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = est_alt_->getCovariance(VELOCITY);
+     515             : 
+     516      166794 :   scope_timer.checkpoint("covariance");
+     517             : 
+     518      166745 :   mrs_lib::set_mutexed(mtx_uav_state_, uav_state, uav_state_);
+     519      166776 :   mrs_lib::set_mutexed(mtx_odom_, odom, odom_);
+     520      166641 :   mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_);
+     521      166817 :   mrs_lib::set_mutexed(mtx_covariance_, pose_covariance, pose_covariance_);
+     522      166735 :   mrs_lib::set_mutexed(mtx_covariance_, twist_covariance, twist_covariance_);
+     523             : }
+     524             : /*//}*/
+     525             : 
+     526             : /*//{ getHeading() */
+     527      262541 : std::optional<double> StateGeneric::getHeading() const {
+     528      262541 :   if (!est_hdg_->isRunning()) {
+     529         138 :     return {};
+     530             :   }
+     531      262396 :   return est_hdg_->getState(POSITION);
+     532             : }
+     533             : /*//}*/
+     534             : 
+     535             : /*//{ setUavState() */
+     536           0 : bool StateGeneric::setUavState([[maybe_unused]] const mrs_msgs::UavState &uav_state) {
+     537             : 
+     538           0 :   if (!isInState(STOPPED_STATE)) {
+     539           0 :     ROS_WARN("[%s]: Estimator state can be set only in the STOPPED state", getPrintName().c_str());
+     540           0 :     return false;
+     541             :   }
+     542             : 
+     543           0 :   ROS_WARN("[%s]: Setting the state of this estimator is not implemented.", getPrintName().c_str());
+     544           0 :   return false;
+     545             : }
+     546             : /*//}*/
+     547             : 
+     548             : }  // namespace mrs_uav_state_estimators
+     549             : 
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html new file mode 100644 index 0000000000..200fa9f980 --- /dev/null +++ b/mrs_uav_state_estimators/src/estimators/state/state_generic.cpp.gcov.overview.html @@ -0,0 +1,158 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_state_estimators/src/estimators/state/state_generic.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_trackers::joy_tracker::JoyTracker::deactivate()18
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>)82
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&)255
mrs_uav_trackers::joy_tracker::JoyTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)281
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&)4
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&)111592
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..b039b8d953 --- /dev/null +++ b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.html @@ -0,0 +1,588 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/joy_tracker - joy_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:6614246.5 %
Date:2024-04-09 22:03:33Functions: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          82 : 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          82 :   this->common_handlers_  = common_handlers;
+     136          82 :   this->private_handlers_ = private_handlers;
+     137             : 
+     138          82 :   _uav_name_ = common_handlers->uav_name;
+     139             : 
+     140          82 :   nh_ = nh;
+     141             : 
+     142          82 :   ros::Time::waitForValid();
+     143             : 
+     144             :   // --------------------------------------------------------------
+     145             :   // |                     loading parameters                     |
+     146             :   // --------------------------------------------------------------
+     147             : 
+     148             :   // | ---------- loading params using the parent's nh ---------- |
+     149             : 
+     150         164 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     151             : 
+     152          82 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     153             : 
+     154          82 :   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          82 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/joy_tracker.yaml");
+     162          82 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/joy_tracker.yaml");
+     163             : 
+     164         164 :   const std::string yaml_prefix = "mrs_uav_trackers/joy_tracker/";
+     165             : 
+     166          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "vertical_tracker/vertical_speed", _vertical_speed_);
+     167             : 
+     168          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "max_tilt", _max_tilt_);
+     169             : 
+     170          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "heading_tracker/heading_rate", _heading_rate_);
+     171             : 
+     172             :   // load channels
+     173          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/pitch", _channel_pitch_);
+     174          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/roll", _channel_roll_);
+     175          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/heading", _channel_heading_);
+     176          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "channels/throttle", _channel_throttle_);
+     177             : 
+     178             :   // load channel multipliers
+     179          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/pitch", _channel_mult_pitch_);
+     180          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/roll", _channel_mult_roll_);
+     181          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/heading", _channel_mult_heading_);
+     182          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "channel_multipliers/throttle", _channel_mult_throttle_);
+     183             : 
+     184          82 :   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          82 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "JoyTracker", _profiler_enabled_);
+     192             : 
+     193             :   // | ----------------------- subscribers ---------------------- |
+     194             : 
+     195          82 :   mrs_lib::SubscribeHandlerOptions shopts;
+     196          82 :   shopts.nh              = nh_;
+     197          82 :   shopts.node_name       = "JoyTracker";
+     198          82 :   shopts.queue_size      = 1;
+     199          82 :   shopts.transport_hints = ros::TransportHints().tcpNoDelay();
+     200             : 
+     201          82 :   sh_joystick_ = mrs_lib::SubscribeHandler<sensor_msgs::Joy>(shopts, "joystick");
+     202             : 
+     203             :   // | --------------------- finish the init -------------------- |
+     204             : 
+     205          82 :   last_update = ros::Time(0);
+     206             : 
+     207          82 :   is_initialized_ = true;
+     208             : 
+     209          82 :   ROS_INFO("[JoyTracker]: initialized");
+     210             : 
+     211          82 :   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          18 : void JoyTracker::deactivate(void) {
+     277             : 
+     278          18 :   is_active_ = false;
+     279             : 
+     280          18 :   ROS_INFO("[JoyTracker]: deactivated");
+     281          18 : }
+     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      111592 : 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      334776 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     300      334776 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("JoyTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     301             : 
+     302             :   {
+     303      111592 :     std::scoped_lock lock(mutex_uav_state_);
+     304             : 
+     305      111592 :     uav_state_ = uav_state;
+     306             : 
+     307      111592 :     got_uav_state_ = true;
+     308             :   }
+     309             : 
+     310      111592 :   double dt = (ros::Time::now() - last_update).toSec();
+     311             : 
+     312      111592 :   last_update = ros::Time::now();
+     313             : 
+     314             :   // up to this part the update() method is evaluated even when the tracker is not active
+     315      111592 :   if (!is_active_) {
+     316      111592 :     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         281 : const std_srvs::SetBoolResponse::ConstPtr JoyTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     388             : 
+     389         562 :   std_srvs::SetBoolResponse res;
+     390         562 :   std::stringstream         ss;
+     391             : 
+     392         281 :   if (cmd->data != callbacks_enabled_) {
+     393             : 
+     394          15 :     callbacks_enabled_ = cmd->data;
+     395             : 
+     396          15 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     397          15 :     ROS_INFO_STREAM_THROTTLE(1.0, "[JoyTracker]: " << ss.str());
+     398             : 
+     399             :   } else {
+     400             : 
+     401         266 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     402         266 :     ROS_WARN_STREAM_THROTTLE(1.0, "[JoyTracker]: " << ss.str());
+     403             :   }
+     404             : 
+     405         281 :   res.message = ss.str();
+     406         281 :   res.success = true;
+     407             : 
+     408         562 :   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         255 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr JoyTracker::setConstraints([
+     465             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     466             : 
+     467         255 :   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           4 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr JoyTracker::setTrajectoryReference([
+     493             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     494           4 :   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          82 : PLUGINLIB_EXPORT_CLASS(mrs_uav_trackers::joy_tracker::JoyTracker, mrs_uav_managers::Tracker)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.overview.html b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.overview.html new file mode 100644 index 0000000000..e3fb54a601 --- /dev/null +++ b/mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp.gcov.overview.html @@ -0,0 +1,146 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/joy_tracker/joy_tracker.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

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

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

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

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

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

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

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
landoff_tracker.cpp +
73.1%73.1%
+ 		73.1 %433 / 59267.7 %21 / 31
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func-sort-c.html b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func-sort-c.html new file mode 100644 index 0000000000..edd21efd77 --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func-sort-c.html @@ -0,0 +1,204 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/landoff_tracker - landoff_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43359273.1 %
Date:2024-04-09 22:03:33Functions: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> >&)4
mrs_uav_trackers::landoff_tracker::LandoffTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)4
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackELand(std_srvs::TriggerRequest_<std::allocator<void> >&, std_srvs::TriggerResponse_<std::allocator<void> >&)7
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackTakeoff(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)19
mrs_uav_trackers::landoff_tracker::LandoffTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)37
mrs_uav_trackers::landoff_tracker::LandoffTracker::deactivate()53
(anonymous namespace)::ProxyExec0::ProxyExec0()82
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>)82
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeState(mrs_uav_trackers::landoff_tracker::States_t)128
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateHorizontal(mrs_uav_trackers::landoff_tracker::States_t)158
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateVertical(mrs_uav_trackers::landoff_tracker::States_t)196
mrs_uav_trackers::landoff_tracker::LandoffTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)255
mrs_uav_trackers::landoff_tracker::LandoffTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)281
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVerticalMotion()637
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontalMotion()637
mrs_uav_trackers::landoff_tracker::LandoffTracker::getStatus()1979
mrs_uav_trackers::landoff_tracker::LandoffTracker::decelerateVertical()4256
mrs_uav_trackers::landoff_tracker::LandoffTracker::accelerateVertical()14104
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontal()18360
mrs_uav_trackers::landoff_tracker::LandoffTracker::timerMain(ros::TimerEvent const&)20950
mrs_uav_trackers::landoff_tracker::LandoffTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)111592
+
+
+ + + +
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..588be3a137 --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.func.html @@ -0,0 +1,204 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/landoff_tracker - landoff_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43359273.1 %
Date:2024-04-09 22:03:33Functions:213167.7 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_trackers::landoff_tracker::LandoffTracker::deactivate()53
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>)82
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeState(mrs_uav_trackers::landoff_tracker::States_t)128
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> >&)4
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> >&)7
mrs_uav_trackers::landoff_tracker::LandoffTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)255
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopHorizontal()18360
mrs_uav_trackers::landoff_tracker::LandoffTracker::callbackTakeoff(mrs_msgs::Vec1Request_<std::allocator<void> >&, mrs_msgs::Vec1Response_<std::allocator<void> >&)19
mrs_uav_trackers::landoff_tracker::LandoffTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)281
mrs_uav_trackers::landoff_tracker::LandoffTracker::accelerateVertical()14104
mrs_uav_trackers::landoff_tracker::LandoffTracker::decelerateVertical()4256
mrs_uav_trackers::landoff_tracker::LandoffTracker::stopVerticalMotion()637
mrs_uav_trackers::landoff_tracker::LandoffTracker::changeStateVertical(mrs_uav_trackers::landoff_tracker::States_t)196
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()637
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)158
mrs_uav_trackers::landoff_tracker::LandoffTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)4
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&)111592
mrs_uav_trackers::landoff_tracker::LandoffTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)37
mrs_uav_trackers::landoff_tracker::LandoffTracker::getStatus()1979
mrs_uav_trackers::landoff_tracker::LandoffTracker::timerMain(ros::TimerEvent const&)20950
+
+
+ + + +
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..dd71e83811 --- /dev/null +++ b/mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp.gcov.html @@ -0,0 +1,1636 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/landoff_tracker/landoff_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/landoff_tracker - landoff_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:43359273.1 %
Date:2024-04-09 22:03:33Functions:213167.7 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

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

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

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

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

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

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

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

Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
line_tracker.cpp +
67.9%67.9%
+ 		67.9 %322 / 47463.3 %19 / 30
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func-sort-c.html b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func-sort-c.html new file mode 100644 index 0000000000..e44ebe63af --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func-sort-c.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker/line_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_tracker - line_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32247467.9 %
Date:2024-04-09 22:03:33Functions: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()182
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&)1716
mrs_uav_trackers::line_tracker::LineTracker::mainTimer(ros::TimerEvent const&)2331
+
+
+ + + +
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..5428b23ffb --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.func.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker/line_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_tracker - line_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32247467.9 %
Date:2024-04-09 22:03:33Functions: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&)1716
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()182
mrs_uav_trackers::line_tracker::LineTracker::mainTimer(ros::TimerEvent const&)2331
+
+
+ + + +
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..a11859fdd4 --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.html @@ -0,0 +1,1359 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker/line_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/line_tracker - line_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:32247467.9 %
Date:2024-04-09 22:03:33Functions: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        1716 : 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        5148 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     519        5148 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("LineTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     520             : 
+     521             :   {
+     522        1716 :     std::scoped_lock lock(mutex_uav_state_);
+     523             : 
+     524        1716 :     uav_state_ = uav_state;
+     525        1716 :     uav_x_     = uav_state_.pose.position.x;
+     526        1716 :     uav_y_     = uav_state_.pose.position.y;
+     527        1716 :     uav_z_     = uav_state_.pose.position.z;
+     528             : 
+     529        1716 :     got_uav_state_ = true;
+     530             :   }
+     531             : 
+     532             :   // up to this part the update() method is evaluated even when the tracker is not active
+     533        1716 :   if (!is_active_) {
+     534          99 :     return {};
+     535             :   }
+     536             : 
+     537        3234 :   mrs_msgs::TrackerCommand tracker_cmd;
+     538             : 
+     539        1617 :   tracker_cmd.header.stamp    = ros::Time::now();
+     540        1617 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     541             : 
+     542             :   {
+     543        1617 :     std::scoped_lock lock(mutex_state_);
+     544             : 
+     545        1617 :     tracker_cmd.position.x = state_x_;
+     546        1617 :     tracker_cmd.position.y = state_y_;
+     547        1617 :     tracker_cmd.position.z = state_z_;
+     548        1617 :     tracker_cmd.heading    = radians::wrap(state_heading_);
+     549             : 
+     550        1617 :     tracker_cmd.velocity.x   = cos(current_heading_) * current_horizontal_speed_;
+     551        1617 :     tracker_cmd.velocity.y   = sin(current_heading_) * current_horizontal_speed_;
+     552        1617 :     tracker_cmd.velocity.z   = current_vertical_direction_ * current_vertical_speed_;
+     553        1617 :     tracker_cmd.heading_rate = speed_heading_;
+     554             : 
+     555        1617 :     tracker_cmd.acceleration.x = 0;
+     556        1617 :     tracker_cmd.acceleration.y = 0;
+     557        1617 :     tracker_cmd.acceleration.z = current_vertical_direction_ * current_vertical_acceleration_;
+     558             : 
+     559        1617 :     tracker_cmd.use_position_vertical   = 1;
+     560        1617 :     tracker_cmd.use_position_horizontal = 1;
+     561        1617 :     tracker_cmd.use_heading             = 1;
+     562        1617 :     tracker_cmd.use_heading_rate        = 1;
+     563        1617 :     tracker_cmd.use_velocity_vertical   = 1;
+     564        1617 :     tracker_cmd.use_velocity_horizontal = 1;
+     565        1617 :     tracker_cmd.use_acceleration        = 1;
+     566             :   }
+     567             : 
+     568        1617 :   return {tracker_cmd};
+     569             : }
+     570             : 
+     571             : //}
+     572             : 
+     573             : /* //{ getStatus() */
+     574             : 
+     575         182 : const mrs_msgs::TrackerStatus LineTracker::getStatus() {
+     576             : 
+     577         182 :   mrs_msgs::TrackerStatus tracker_status;
+     578             : 
+     579         182 :   tracker_status.active            = is_active_;
+     580         182 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     581             : 
+     582         182 :   bool idling = current_state_vertical_ == IDLE_STATE && current_state_horizontal_ == IDLE_STATE;
+     583             : 
+     584         182 :   tracker_status.have_goal = !idling;
+     585             : 
+     586         182 :   tracker_status.tracking_trajectory = false;
+     587             : 
+     588         182 :   return tracker_status;
+     589             : }
+     590             : 
+     591             : //}
+     592             : 
+     593             : /* //{ enableCallbacks() */
+     594             : 
+     595           0 : const std_srvs::SetBoolResponse::ConstPtr LineTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     596             : 
+     597           0 :   std_srvs::SetBoolResponse res;
+     598           0 :   std::stringstream         ss;
+     599             : 
+     600           0 :   if (cmd->data != callbacks_enabled_) {
+     601             : 
+     602           0 :     callbacks_enabled_ = cmd->data;
+     603             : 
+     604           0 :     ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+     605           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[LineTracker]: " << ss.str());
+     606             : 
+     607             :   } else {
+     608             : 
+     609           0 :     ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled");
+     610           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[LineTracker]: " << ss.str());
+     611             :   }
+     612             : 
+     613           0 :   res.message = ss.str();
+     614           0 :   res.success = true;
+     615             : 
+     616           0 :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+     617             : }
+     618             : 
+     619             : //}
+     620             : 
+     621             : /* switchOdometrySource() //{ */
+     622             : 
+     623           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::switchOdometrySource(const mrs_msgs::UavState &new_uav_state) {
+     624             : 
+     625           0 :   std::scoped_lock lock(mutex_goal_, mutex_state_);
+     626             : 
+     627           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     628             : 
+     629           0 :   double old_heading  = 0;
+     630           0 :   double new_heading  = 0;
+     631           0 :   bool   got_headings = true;
+     632             : 
+     633             :   try {
+     634           0 :     old_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     635             :   }
+     636           0 :   catch (...) {
+     637           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the old UAV heading");
+     638           0 :     got_headings = false;
+     639             :   }
+     640             : 
+     641             :   try {
+     642           0 :     new_heading = mrs_lib::AttitudeConverter(new_uav_state.pose.orientation).getHeading();
+     643             :   }
+     644           0 :   catch (...) {
+     645           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the new UAV heading");
+     646           0 :     got_headings = false;
+     647             :   }
+     648             : 
+     649           0 :   std_srvs::TriggerResponse res;
+     650             : 
+     651           0 :   if (!got_headings) {
+     652           0 :     res.message = "could not calculate the heading difference";
+     653           0 :     res.success = false;
+     654             : 
+     655           0 :     return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     656             :   }
+     657             : 
+     658             :   // | --------- recalculate the goal to new coordinates -------- |
+     659             : 
+     660           0 :   double dx       = new_uav_state.pose.position.x - uav_state.pose.position.x;
+     661           0 :   double dy       = new_uav_state.pose.position.y - uav_state.pose.position.y;
+     662           0 :   double dz       = new_uav_state.pose.position.z - uav_state.pose.position.z;
+     663           0 :   double dheading = new_heading - old_heading;
+     664             : 
+     665           0 :   goal_x_ += dx;
+     666           0 :   goal_y_ += dy;
+     667           0 :   goal_z_ += dz;
+     668           0 :   goal_heading_ += dheading;
+     669             : 
+     670             :   // | -------------------- update the state -------------------- |
+     671             : 
+     672           0 :   state_x_ += dx;
+     673           0 :   state_y_ += dy;
+     674           0 :   state_z_ += dz;
+     675           0 :   state_heading_ += dheading;
+     676             : 
+     677           0 :   current_heading_ = atan2(goal_y_ - state_y_, goal_x_ - state_x_);
+     678             : 
+     679           0 :   res.message = "odometry source switched";
+     680           0 :   res.success = true;
+     681             : 
+     682           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     683             : }
+     684             : 
+     685             : //}
+     686             : 
+     687             : /* //{ hover() */
+     688             : 
+     689           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     690             : 
+     691           0 :   std_srvs::TriggerResponse res;
+     692             : 
+     693             :   // --------------------------------------------------------------
+     694             :   // |          horizontal initial conditions prediction          |
+     695             :   // --------------------------------------------------------------
+     696             :   {
+     697           0 :     std::scoped_lock lock(mutex_state_, mutex_uav_state_);
+     698             : 
+     699           0 :     current_horizontal_speed_ = sqrt(pow(uav_state_.velocity.linear.x, 2) + pow(uav_state_.velocity.linear.y, 2));
+     700           0 :     current_vertical_speed_   = uav_state_.velocity.linear.z;
+     701           0 :     current_heading_          = atan2(uav_state_.velocity.linear.y, uav_state_.velocity.linear.x);
+     702             :   }
+     703             : 
+     704             :   double horizontal_t_stop, horizontal_stop_dist, stop_dist_x, stop_dist_y;
+     705             : 
+     706             :   {
+     707           0 :     std::scoped_lock lock(mutex_state_);
+     708             : 
+     709           0 :     horizontal_t_stop    = current_horizontal_speed_ / _horizontal_acceleration_;
+     710           0 :     horizontal_stop_dist = (horizontal_t_stop * current_horizontal_speed_) / 2.0;
+     711           0 :     stop_dist_x          = cos(current_heading_) * horizontal_stop_dist;
+     712           0 :     stop_dist_y          = sin(current_heading_) * horizontal_stop_dist;
+     713             :   }
+     714             : 
+     715             :   // --------------------------------------------------------------
+     716             :   // |           vertical initial conditions prediction           |
+     717             :   // --------------------------------------------------------------
+     718             : 
+     719             :   double vertical_t_stop, vertical_stop_dist;
+     720             : 
+     721             :   {
+     722           0 :     std::scoped_lock lock(mutex_state_);
+     723             : 
+     724           0 :     vertical_t_stop    = current_vertical_speed_ / _vertical_acceleration_;
+     725           0 :     vertical_stop_dist = current_vertical_direction_ * (vertical_t_stop * current_vertical_speed_) / 2.0;
+     726             :   }
+     727             : 
+     728             :   // --------------------------------------------------------------
+     729             :   // |                        set the goal                        |
+     730             :   // --------------------------------------------------------------
+     731             : 
+     732             :   {
+     733           0 :     std::scoped_lock lock(mutex_goal_, mutex_state_);
+     734             : 
+     735           0 :     goal_x_ = state_x_ + stop_dist_x;
+     736           0 :     goal_y_ = state_y_ + stop_dist_y;
+     737           0 :     goal_z_ = state_z_ + vertical_stop_dist;
+     738             :   }
+     739             : 
+     740           0 :   res.message = "hover initiated";
+     741           0 :   res.success = true;
+     742             : 
+     743           0 :   changeState(STOP_MOTION_STATE);
+     744             : 
+     745           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+     746             : }
+     747             : 
+     748             : //}
+     749             : 
+     750             : /* //{ startTrajectoryTracking() */
+     751             : 
+     752           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     753           0 :   return std_srvs::TriggerResponse::Ptr();
+     754             : }
+     755             : 
+     756             : //}
+     757             : 
+     758             : /* //{ stopTrajectoryTracking() */
+     759             : 
+     760           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     761           0 :   return std_srvs::TriggerResponse::Ptr();
+     762             : }
+     763             : 
+     764             : //}
+     765             : 
+     766             : /* //{ resumeTrajectoryTracking() */
+     767             : 
+     768           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     769           0 :   return std_srvs::TriggerResponse::Ptr();
+     770             : }
+     771             : 
+     772             : //}
+     773             : 
+     774             : /* //{ gotoTrajectoryStart() */
+     775             : 
+     776           0 : const std_srvs::TriggerResponse::ConstPtr LineTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) {
+     777           0 :   return std_srvs::TriggerResponse::Ptr();
+     778             : }
+     779             : 
+     780             : //}
+     781             : 
+     782             : /* //{ setConstraints() */
+     783             : 
+     784           3 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr LineTracker::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     785             : 
+     786           6 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     787             : 
+     788             :   // this is the place to copy the constraints
+     789             :   {
+     790           3 :     std::scoped_lock lock(mutex_constraints_);
+     791             : 
+     792           3 :     _horizontal_speed_        = cmd->constraints.horizontal_speed;
+     793           3 :     _horizontal_acceleration_ = cmd->constraints.horizontal_acceleration;
+     794             : 
+     795           3 :     _vertical_speed_        = cmd->constraints.vertical_ascending_speed;
+     796           3 :     _vertical_acceleration_ = cmd->constraints.vertical_ascending_acceleration;
+     797             : 
+     798           3 :     _heading_rate_ = cmd->constraints.heading_speed;
+     799             :   }
+     800             : 
+     801           3 :   res.success = true;
+     802           3 :   res.message = "constraints updated";
+     803             : 
+     804           6 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+     805             : }
+     806             : 
+     807             : //}
+     808             : 
+     809             : /* //{ setReference() */
+     810             : 
+     811           1 : const mrs_msgs::ReferenceSrvResponse::ConstPtr LineTracker::setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd) {
+     812             : 
+     813           2 :   mrs_msgs::ReferenceSrvResponse res;
+     814             : 
+     815           1 :   auto state_heading = mrs_lib::get_mutexed(mutex_state_, state_heading_);
+     816             : 
+     817             :   {
+     818           1 :     std::scoped_lock lock(mutex_goal_);
+     819             : 
+     820           1 :     goal_x_       = cmd->reference.position.x;
+     821           1 :     goal_y_       = cmd->reference.position.y;
+     822           1 :     goal_z_       = cmd->reference.position.z;
+     823           1 :     goal_heading_ = radians::unwrap(cmd->reference.heading, state_heading);
+     824             : 
+     825           1 :     ROS_INFO("[LineTracker]: received new setpoint %.2f, %.2f, %.2f, %.2f", goal_x_, goal_y_, goal_z_, goal_heading_);
+     826             : 
+     827           1 :     have_goal_ = true;
+     828             :   }
+     829             : 
+     830           1 :   changeState(STOP_MOTION_STATE);
+     831             : 
+     832           1 :   res.success = true;
+     833           1 :   res.message = "reference set";
+     834             : 
+     835           2 :   return mrs_msgs::ReferenceSrvResponse::ConstPtr(new mrs_msgs::ReferenceSrvResponse(res));
+     836             : }
+     837             : 
+     838             : //}
+     839             : 
+     840             : /* //{ setVelocityReference() */
+     841             : 
+     842           0 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr LineTracker::setVelocityReference([
+     843             :     [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd) {
+     844           0 :   return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+     845             : }
+     846             : 
+     847             : //}
+     848             : 
+     849             : /* //{ setTrajectoryReference() */
+     850             : 
+     851           0 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr LineTracker::setTrajectoryReference([
+     852             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     853           0 :   return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr();
+     854             : }
+     855             : 
+     856             : //}
+     857             : 
+     858             : // | ----------------- state machine routines ----------------- |
+     859             : 
+     860             : /* //{ changeStateHorizontal() */
+     861             : 
+     862           8 : void LineTracker::changeStateHorizontal(States_t new_state) {
+     863             : 
+     864           8 :   previous_state_horizontal_ = current_state_horizontal_;
+     865           8 :   current_state_horizontal_  = new_state;
+     866             : 
+     867             :   // just for ROS_INFO
+     868           8 :   ROS_DEBUG("[LineTracker]: Switching horizontal state %s -> %s", state_names[previous_state_horizontal_], state_names[current_state_horizontal_]);
+     869           8 : }
+     870             : 
+     871             : //}
+     872             : 
+     873             : /* //{ changeStateVertical() */
+     874             : 
+     875           8 : void LineTracker::changeStateVertical(States_t new_state) {
+     876             : 
+     877           8 :   previous_state_vertical_ = current_state_vertical_;
+     878           8 :   current_state_vertical_  = new_state;
+     879             : 
+     880             :   // just for ROS_INFO
+     881           8 :   ROS_DEBUG("[LineTracker]: Switching vertical state %s -> %s", state_names[previous_state_vertical_], state_names[current_state_vertical_]);
+     882           8 : }
+     883             : 
+     884             : //}
+     885             : 
+     886             : /* //{ changeState() */
+     887             : 
+     888           6 : void LineTracker::changeState(States_t new_state) {
+     889             : 
+     890           6 :   changeStateVertical(new_state);
+     891           6 :   changeStateHorizontal(new_state);
+     892           6 : }
+     893             : 
+     894             : //}
+     895             : 
+     896             : // | --------------------- motion routines -------------------- |
+     897             : 
+     898             : /* //{ stopHorizontalMotion() */
+     899             : 
+     900           2 : void LineTracker::stopHorizontalMotion(void) {
+     901             : 
+     902             :   {
+     903           4 :     std::scoped_lock lock(mutex_state_);
+     904             : 
+     905           2 :     current_horizontal_speed_ -= _horizontal_acceleration_ * _tracker_dt_;
+     906             : 
+     907           2 :     if (current_horizontal_speed_ < 0) {
+     908           2 :       current_horizontal_speed_        = 0;
+     909           2 :       current_horizontal_acceleration_ = 0;
+     910             :     } else {
+     911           0 :       current_horizontal_acceleration_ = -_horizontal_acceleration_;
+     912             :     }
+     913             :   }
+     914           2 : }
+     915             : 
+     916             : //}
+     917             : 
+     918             : /* //{ stopVerticalMotion() */
+     919             : 
+     920           2 : void LineTracker::stopVerticalMotion(void) {
+     921             : 
+     922             :   {
+     923           4 :     std::scoped_lock lock(mutex_state_);
+     924             : 
+     925           2 :     current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+     926             : 
+     927           2 :     if (current_vertical_speed_ < 0) {
+     928           2 :       current_vertical_speed_        = 0;
+     929           2 :       current_vertical_acceleration_ = 0;
+     930             :     } else {
+     931           0 :       current_vertical_acceleration_ = -_vertical_acceleration_;
+     932             :     }
+     933             :   }
+     934           2 : }
+     935             : 
+     936             : //}
+     937             : 
+     938             : /* //{ accelerateHorizontal() */
+     939             : 
+     940        1004 : void LineTracker::accelerateHorizontal(void) {
+     941             : 
+     942             :   // copy member variables
+     943        1004 :   auto [goal_x, goal_y]                             = mrs_lib::get_mutexed(mutex_goal_, goal_x_, goal_y_);
+     944        1004 :   auto [state_x, state_y, current_horizontal_speed] = mrs_lib::get_mutexed(mutex_state_, state_x_, state_y_, current_horizontal_speed_);
+     945             : 
+     946             :   {
+     947        1004 :     std::scoped_lock lock(mutex_state_);
+     948             : 
+     949        1004 :     current_heading_ = atan2(goal_y - state_y, goal_x - state_x);
+     950             :   }
+     951             : 
+     952        1004 :   auto current_heading = mrs_lib::get_mutexed(mutex_state_, current_heading_);
+     953             : 
+     954             :   double horizontal_t_stop, horizontal_stop_dist, stop_dist_x, stop_dist_y;
+     955             : 
+     956        1004 :   horizontal_t_stop    = current_horizontal_speed / _horizontal_acceleration_;
+     957        1004 :   horizontal_stop_dist = (horizontal_t_stop * current_horizontal_speed) / 2.0;
+     958        1004 :   stop_dist_x          = cos(current_heading) * horizontal_stop_dist;
+     959        1004 :   stop_dist_y          = sin(current_heading) * horizontal_stop_dist;
+     960             : 
+     961             :   {
+     962        2008 :     std::scoped_lock lock(mutex_state_);
+     963             : 
+     964        1004 :     current_horizontal_speed_ += _horizontal_acceleration_ * _tracker_dt_;
+     965             : 
+     966        1004 :     if (current_horizontal_speed_ >= _horizontal_speed_) {
+     967         905 :       current_horizontal_speed_        = _horizontal_speed_;
+     968         905 :       current_horizontal_acceleration_ = 0;
+     969             :     } else {
+     970          99 :       current_horizontal_acceleration_ = _horizontal_acceleration_;
+     971             :     }
+     972             :   }
+     973             : 
+     974        1004 :   if (sqrt(pow(state_x + stop_dist_x - goal_x, 2) + pow(state_y + stop_dist_y - goal_y, 2)) < (2 * (_horizontal_speed_ * _tracker_dt_))) {
+     975             : 
+     976             :     {
+     977           1 :       std::scoped_lock lock(mutex_state_);
+     978             : 
+     979           1 :       current_horizontal_acceleration_ = 0;
+     980             :     }
+     981             : 
+     982           1 :     changeStateHorizontal(DECELERATING_STATE);
+     983             :   }
+     984        1004 : }
+     985             : 
+     986             : //}
+     987             : 
+     988             : /* //{ accelerateVertical() */
+     989             : 
+     990         199 : void LineTracker::accelerateVertical(void) {
+     991             : 
+     992         199 :   auto goal_z                            = mrs_lib::get_mutexed(mutex_goal_, goal_z_);
+     993         199 :   auto [state_z, current_vertical_speed] = mrs_lib::get_mutexed(mutex_state_, state_z_, current_vertical_speed_);
+     994             : 
+     995             :   // set the right heading
+     996         199 :   double tar_z = goal_z - state_z;
+     997             : 
+     998             :   // set the right vertical direction
+     999             :   {
+    1000         199 :     std::scoped_lock lock(mutex_state_);
+    1001             : 
+    1002         199 :     current_vertical_direction_ = mrs_lib::signum(tar_z);
+    1003             :   }
+    1004             : 
+    1005         199 :   auto current_vertical_direction = mrs_lib::get_mutexed(mutex_state_, current_vertical_direction_);
+    1006             : 
+    1007             :   // calculate the time to stop and the distance it will take to stop [vertical]
+    1008         199 :   double vertical_t_stop    = current_vertical_speed / _vertical_acceleration_;
+    1009         199 :   double vertical_stop_dist = (vertical_t_stop * current_vertical_speed) / 2.0;
+    1010         199 :   double stop_dist_z        = current_vertical_direction * vertical_stop_dist;
+    1011             : 
+    1012             :   {
+    1013         398 :     std::scoped_lock lock(mutex_state_);
+    1014             : 
+    1015         199 :     current_vertical_speed_ += _vertical_acceleration_ * _tracker_dt_;
+    1016             : 
+    1017         199 :     if (current_vertical_speed_ >= _vertical_speed_) {
+    1018         100 :       current_vertical_speed_        = _vertical_speed_;
+    1019         100 :       current_vertical_acceleration_ = 0;
+    1020             :     } else {
+    1021          99 :       current_vertical_acceleration_ = _vertical_acceleration_;
+    1022             :     }
+    1023             :   }
+    1024             : 
+    1025         199 :   if (fabs(state_z + stop_dist_z - goal_z) < (2 * (_vertical_speed_ * _tracker_dt_))) {
+    1026             : 
+    1027             :     {
+    1028           1 :       std::scoped_lock lock(mutex_state_);
+    1029             : 
+    1030           1 :       current_vertical_acceleration_ = 0;
+    1031             :     }
+    1032             : 
+    1033           1 :     changeStateVertical(DECELERATING_STATE);
+    1034             :   }
+    1035         199 : }
+    1036             : 
+    1037             : //}
+    1038             : 
+    1039             : /* //{ decelerateHorizontal() */
+    1040             : 
+    1041         100 : void LineTracker::decelerateHorizontal(void) {
+    1042             : 
+    1043             :   {
+    1044         200 :     std::scoped_lock lock(mutex_state_);
+    1045             : 
+    1046         100 :     current_horizontal_speed_ -= _horizontal_acceleration_ * _tracker_dt_;
+    1047             : 
+    1048         100 :     if (current_horizontal_speed_ < 0) {
+    1049           1 :       current_horizontal_speed_ = 0;
+    1050             :     } else {
+    1051          99 :       current_horizontal_acceleration_ = -_horizontal_acceleration_;
+    1052             :     }
+    1053             :   }
+    1054             : 
+    1055         100 :   auto current_horizontal_speed = mrs_lib::get_mutexed(mutex_state_, current_horizontal_speed_);
+    1056             : 
+    1057         100 :   if (current_horizontal_speed == 0) {
+    1058             : 
+    1059             :     {
+    1060           1 :       std::scoped_lock lock(mutex_state_);
+    1061             : 
+    1062           1 :       current_horizontal_acceleration_ = 0;
+    1063             :     }
+    1064             : 
+    1065           1 :     changeStateHorizontal(STOPPING_STATE);
+    1066             :   }
+    1067         100 : }
+    1068             : 
+    1069             : //}
+    1070             : 
+    1071             : /* //{ decelerateVertical() */
+    1072             : 
+    1073         100 : void LineTracker::decelerateVertical(void) {
+    1074             : 
+    1075             :   {
+    1076         200 :     std::scoped_lock lock(mutex_state_);
+    1077             : 
+    1078         100 :     current_vertical_speed_ -= _vertical_acceleration_ * _tracker_dt_;
+    1079             : 
+    1080         100 :     if (current_vertical_speed_ < 0) {
+    1081           1 :       current_vertical_speed_ = 0;
+    1082             :     } else {
+    1083          99 :       current_vertical_acceleration_ = -_vertical_acceleration_;
+    1084             :     }
+    1085             :   }
+    1086             : 
+    1087         100 :   auto current_vertical_speed = mrs_lib::get_mutexed(mutex_state_, current_vertical_speed_);
+    1088             : 
+    1089         100 :   if (current_vertical_speed == 0) {
+    1090           1 :     current_vertical_acceleration_ = 0;
+    1091           1 :     changeStateVertical(STOPPING_STATE);
+    1092             :   }
+    1093         100 : }
+    1094             : 
+    1095             : //}
+    1096             : 
+    1097             : /* //{ stopHorizontal() */
+    1098             : 
+    1099          46 : void LineTracker::stopHorizontal(void) {
+    1100             : 
+    1101             :   {
+    1102          46 :     std::scoped_lock lock(mutex_state_);
+    1103             : 
+    1104          46 :     state_x_                         = 0.95 * state_x_ + 0.05 * goal_x_;
+    1105          46 :     state_y_                         = 0.95 * state_y_ + 0.05 * goal_y_;
+    1106          46 :     current_horizontal_acceleration_ = 0;
+    1107             :   }
+    1108          46 : }
+    1109             : 
+    1110             : //}
+    1111             : 
+    1112             : /* //{ stopVertical() */
+    1113             : 
+    1114         851 : void LineTracker::stopVertical(void) {
+    1115             : 
+    1116             :   {
+    1117         851 :     std::scoped_lock lock(mutex_state_);
+    1118             : 
+    1119         851 :     state_z_                       = 0.95 * state_z_ + 0.05 * goal_z_;
+    1120         851 :     current_vertical_acceleration_ = 0;
+    1121             :   }
+    1122         851 : }
+    1123             : 
+    1124             : //}
+    1125             : 
+    1126             : // | ------------------------- timers ------------------------- |
+    1127             : 
+    1128             : /* //{ mainTimer() */
+    1129             : 
+    1130        2331 : void LineTracker::mainTimer(const ros::TimerEvent &event) {
+    1131             : 
+    1132        2331 :   if (!is_active_) {
+    1133         512 :     return;
+    1134             :   }
+    1135             : 
+    1136        5457 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("main", _tracker_loop_rate_, 0.01, event);
+    1137        5457 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("LineTracker::main", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1138             : 
+    1139        1819 :   auto [goal_x, goal_y, goal_z]    = mrs_lib::get_mutexed(mutex_goal_, goal_x_, goal_y_, goal_z_);
+    1140        1819 :   auto [state_x, state_y, state_z] = mrs_lib::get_mutexed(mutex_state_, state_x_, state_y_, state_z_);
+    1141             : 
+    1142        1819 :   switch (current_state_horizontal_) {
+    1143             : 
+    1144         667 :     case IDLE_STATE:
+    1145             : 
+    1146         667 :       break;
+    1147             : 
+    1148           2 :     case STOP_MOTION_STATE:
+    1149             : 
+    1150           2 :       stopHorizontalMotion();
+    1151             : 
+    1152           2 :       break;
+    1153             : 
+    1154        1004 :     case ACCELERATING_STATE:
+    1155             : 
+    1156        1004 :       accelerateHorizontal();
+    1157             : 
+    1158        1004 :       break;
+    1159             : 
+    1160         100 :     case DECELERATING_STATE:
+    1161             : 
+    1162         100 :       decelerateHorizontal();
+    1163             : 
+    1164         100 :       break;
+    1165             : 
+    1166          46 :     case STOPPING_STATE:
+    1167             : 
+    1168          46 :       stopHorizontal();
+    1169             : 
+    1170          46 :       break;
+    1171             :   }
+    1172             : 
+    1173        1819 :   switch (current_state_vertical_) {
+    1174             : 
+    1175         667 :     case IDLE_STATE:
+    1176             : 
+    1177         667 :       break;
+    1178             : 
+    1179           2 :     case STOP_MOTION_STATE:
+    1180             : 
+    1181           2 :       stopVerticalMotion();
+    1182             : 
+    1183           2 :       break;
+    1184             : 
+    1185         199 :     case ACCELERATING_STATE:
+    1186             : 
+    1187         199 :       accelerateVertical();
+    1188             : 
+    1189         199 :       break;
+    1190             : 
+    1191         100 :     case DECELERATING_STATE:
+    1192             : 
+    1193         100 :       decelerateVertical();
+    1194             : 
+    1195         100 :       break;
+    1196             : 
+    1197         851 :     case STOPPING_STATE:
+    1198             : 
+    1199         851 :       stopVertical();
+    1200             : 
+    1201         851 :       break;
+    1202             :   }
+    1203             : 
+    1204        1819 :   if (current_state_horizontal_ == STOP_MOTION_STATE && current_state_vertical_ == STOP_MOTION_STATE) {
+    1205           2 :     if (current_vertical_speed_ == 0 && current_horizontal_speed_ == 0) {
+    1206           2 :       if (have_goal_) {
+    1207           1 :         changeState(ACCELERATING_STATE);
+    1208             :       } else {
+    1209           1 :         changeState(STOPPING_STATE);
+    1210             :       }
+    1211             :     }
+    1212             :   }
+    1213             : 
+    1214        1819 :   if (current_state_horizontal_ == STOPPING_STATE && current_state_vertical_ == STOPPING_STATE) {
+    1215          48 :     if (fabs(state_x - goal_x) < 1e-3 && fabs(state_y - goal_y) < 1e-3 && fabs(state_z - goal_z) < 1e-3) {
+    1216             : 
+    1217             :       {
+    1218           2 :         std::scoped_lock lock(mutex_state_);
+    1219             : 
+    1220           2 :         state_x_ = goal_x;
+    1221           2 :         state_y_ = goal_y;
+    1222           2 :         state_z_ = goal_z;
+    1223             :       }
+    1224             : 
+    1225           2 :       changeState(IDLE_STATE);
+    1226             : 
+    1227           2 :       have_goal_ = false;
+    1228             :     }
+    1229             :   }
+    1230             : 
+    1231             :   {
+    1232        1819 :     std::scoped_lock lock(mutex_state_);
+    1233             : 
+    1234        1819 :     state_x_ += cos(current_heading_) * current_horizontal_speed_ * _tracker_dt_;
+    1235        1819 :     state_y_ += sin(current_heading_) * current_horizontal_speed_ * _tracker_dt_;
+    1236        1819 :     state_z_ += current_vertical_direction_ * current_vertical_speed_ * _tracker_dt_;
+    1237             :   }
+    1238             : 
+    1239             :   // --------------------------------------------------------------
+    1240             :   // |                        heading tracking                        |
+    1241             :   // --------------------------------------------------------------
+    1242             : 
+    1243             :   {
+    1244        3638 :     std::scoped_lock lock(mutex_state_);
+    1245             : 
+    1246             :     // compute the desired heading rate
+    1247             :     double current_heading_rate;
+    1248        1819 :     if (fabs(goal_heading_ - state_heading_) > M_PI)
+    1249           0 :       current_heading_rate = -_heading_gain_ * (goal_heading_ - state_heading_);
+    1250             :     else
+    1251        1819 :       current_heading_rate = _heading_gain_ * (goal_heading_ - state_heading_);
+    1252             : 
+    1253        1819 :     if (current_heading_rate > _heading_rate_) {
+    1254          50 :       current_heading_rate = _heading_rate_;
+    1255        1769 :     } else if (current_heading_rate < -_heading_rate_) {
+    1256           0 :       current_heading_rate = -_heading_rate_;
+    1257             :     }
+    1258             : 
+    1259             :     // flap the resulted state_heading_ aroud PI
+    1260        1819 :     state_heading_ += current_heading_rate * _tracker_dt_;
+    1261             : 
+    1262        1819 :     if (fabs(state_heading_ - goal_heading_) < (2 * (_heading_rate_ * _tracker_dt_))) {
+    1263        1380 :       state_heading_ = goal_heading_;
+    1264             :     }
+    1265             :   }
+    1266             : }
+    1267             : 
+    1268             : //}
+    1269             : 
+    1270             : }  // namespace line_tracker
+    1271             : 
+    1272             : }  // namespace mrs_uav_trackers
+    1273             : 
+    1274             : #include <pluginlib/class_list_macros.h>
+    1275           1 : PLUGINLIB_EXPORT_CLASS(mrs_uav_trackers::line_tracker::LineTracker, mrs_uav_managers::Tracker)
+
+
+
+ + + + +
Generated by: LCOV version 1.14
+
+ + + diff --git a/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.overview.html b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.overview.html new file mode 100644 index 0000000000..b00a836f88 --- /dev/null +++ b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.overview.html @@ -0,0 +1,339 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/line_tracker/line_tracker.cpp + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
+ Top

+ Overview +
+ + diff --git a/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.png b/mrs_uav_trackers/src/line_tracker/line_tracker.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..b7a7e459ce10c0e0990050079801959c58954678 GIT binary patch literal 3774 zcmV;v4ngsWP)QIT0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vph4P2>B& zTdVS>cMzRYBt0+*&1P-O*py;~8Up6kQ6O7EQ45SwRTV~)J=uH8KK8!)@mpYX^T|@Gg*&A(r#>5x8vQRudPC+tgks|$T|+}z42@_4JM_fS?;Ig4wQ-!tFgsA7 zmaeBW`rp9_KEXeTmMm#dJCN1^6bV)PMJS;FtQejc3CA0$=y=v~L>Tbblp+K=3iM?X zXlsxqOi0mE0MdxS0}T2n3Y-wIwvL2iiP69^f}c@g#_+8QIB0=8QUM%I9rZ$M(Vjs@ zn`$UVim6^2IOa)xgH|~<86ZIGHyUmZfw-}N(g%Q6tWnguPx7(X%4g&LHZ{R_-wnQd zSh%C3n(8X+s0!jb0M+Gezcbv7_QIrfgC3Y-?S1+X#CtKp)SnlbuFa=_Ax} z6Dfw3rNB`OoYcUv)@_7&A%S3!S))4M@5>#P#r zLz&Tlc9gVsVKiLbGn9^3rW}fV)fNhTJYJw|`~UAB&-Z`l2M<$D;1hfzl-nbPu)ypt zoLm%qdj0aLz1ZNSWB@bz`t&&F=SoSf7MMX%Du{Xp@Q-x^)=M_U_xOQ_5IO)9)^V{v zZS9IFzF#a$d{pWMJlZD(RF>=15h#`lxXv+P5$lLl0Z?kEQr5EAWfWIqUuo7+UFdmP^cl@&s;-?P&I#Y_6<2B4x5w#b zll|kg$2jAseZgzbhm=CwA+xjBg}lJ+mSL{IUe`$DXQ-L+d}m@PvIk+Fb=j1IHD?NJ z9kswU;h#?6-jAmvK?6LeW_qAXfAq>Dwj+`6!_D&pQgbzSQU`+59Kc@3ml>Jni{7)`Bw@Xaq6; zjxL1%EJa~;(N`l%@eU>>IiZ5iqv(qw*Nu4FuwJ|ah7Ebd7p3JAG}+*n6>F5rgs?NLj-2Bwo*iZnG}s6$4q5A z{vEG3)0sErpz!E)$}cNhczo#BuO~M-?zw)APY02gB@^X}SzK*C+~;v-9I4=UH|W9_ zF!95_e{sNLP=hq!Ysgvw)K2X)x-e7C>=HLv<{oj+byc&mf($5Mpk_qN(3=(MBMqG0 z+w}}SKCcuvdo#ef?+fmr88a!W%SZD)*4|B5@I6oi zvW)9#4VD%-0n}5}FWPC>;!MwM;rtsHeS_z$#Rcu*c1w6Id3|(Ktk#iuVV>lLw|678 zvW`NgNm#1v0DPr#S0%!nWs77!vVab(0Un~nfVy%Eb5|R2L7>6Jj ziXqR8#+_?wJ%#?(_+^ z(4MISm_P{f6q|>lnq=fiTkj%+!kl|+P}BPqZ-y|PQY5B5a_ZVad__;$Q;g4a);9~JfW9vG&VrTYM1c7H;PvCzj9Px$-! zB2$7~iY6j`$!BQ)BGLrK(<3!}43sV+-MY+%)eZi^Ljx6iK72D4NiCkU0Xn>Gbf&ol zV*^L=crSK*m(lfOXDXPUVRf>kXEw5Z2ERi+{0d<|JEfL-pV$g;x! z>r6#QH6vPv-fUdTE_{3F+&3tNeK*oa)^t>g`h9!MF@Jp*p7x$P@Lmk$6|HYt@hPhJ zqgxOMOM^{_uiqCIx>6q>>Jyb*J95o)!FFtr@%?MZHVygLj%{Xr|9v|)+89gk*hB00 z^*akZsbGyh?`BSu!yMcuNcL*Ey1lNAHht~y!$xI*cWI07TF9E`Y`-B}w8VbTx=zM6 zBi+qRft~h|rf&Ll`nbX?TqI$Xb%gs;xc0LzF19bF)mD4Kr35#bmAe$in;2jQMY~lx z+=mZz_-D2P*KUBG4UPE4O5K2W!JB|>cQq&D*OSl!{?EFC2^>R60Q+usmpXdR-({EU z_9CLUaC^A(FXr}IxjsJrJc^J^-Q+*nzY?eRFM4?QVxeotDJj?Dss=x|NyrV3TKpmn z?)RJeMI@I@u6g2DvuT{88Ei7ZVk&G#G8t+LqRDVJBLa(U2AhmCg+H75+HofH(KqWo zh$cub#XTeEPH!urKy@9z!LIFnn>NN>(Y2R-GI0y$4cpSom($m+cLGW`9MhXB&|`#! z8;?8y(#jZ$I;q>=nhu|w zw)kK6K^pE?hMH+BIV<%nM=A7Os1-)rj_e{kw?3nFq?d~TzBFn!H#MLS!vSZ(13Pc8 zhbd$}z|@`piAil1auzr4ioz7J9%BRFw~PwpG7SXm8GMAifs}a-KKavqqs=i6=0>o= z9u~oOXh6O=#K5b2zJl};dZI-% zDxg4N@tSE~T{KjXyHG2PwjJ3}fn)t^Lxpcm7JP}@`)pF0yMN69TTOiKo}x~e9^Q=R zAA>3%KXU?2+-b3lrc&x}@{gaLO0jkPwf^x-r&8bIA3wSPA-TtP(cT>Zzr;U^?y=ZE zitaI(7mD|=vYvtm{!&lT?8r%&52=&-=lqr!Z8B^Q`h$lt1F?T#S?Ck`s5 z{w6i!l_$rW{Vwgn$A-~@T-~mwhF2V8eS{AjFWuH?QzXfrxu?zx7+}8>5RoTe?C;5w zFAgqYI{cn_5Yer%-?^*m&G+FNyRbrBb5x?%_slLrN&?js=OndUiZF%Th9YyTd;^L) zpq66F&F546lluFmZbflUi(Eia1MK;bgP7O*b9-sl8cX;dsoOh%KdCDz>VR5`{oG#a z7;7Fy4X}gTlQxGfXp;f6HjDuK2dlmN!9E)+Ju2Kuk$|dnAwy9Hs#@} + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-09 22:03:33Functions: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..3115c64024 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-09 22:03:33Functions: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..f0fa42b40e --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-09 22:03:33Functions: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..dcac09d384 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-09 22:03:33Functions: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..7478b3a133 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-09 22:03:33Functions: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..fee90b19cc --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_trackerHitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-09 22:03:33Functions: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..f9f5f4f661 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func-sort-c.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_tracker - midair_activation_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-09 22:03:33Functions: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&)4
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::getStatus()39
(anonymous namespace)::ProxyExec0::ProxyExec0()82
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>)82
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)111
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::deactivate()129
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)255
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)281
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)111592
+
+
+ + + +
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..bac9984e56 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.func.html @@ -0,0 +1,152 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp - functions + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_tracker - midair_activation_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-09 22:03:33Functions:91850.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::deactivate()129
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>)82
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&)255
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)281
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&)4
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&)111592
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)111
mrs_uav_trackers::midair_activation_tracker::MidairActivationTracker::getStatus()39
+
+
+ + + +
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..9eec2eb967 --- /dev/null +++ b/mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp.gcov.html @@ -0,0 +1,426 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/midair_activation_tracker/midair_activation_tracker.cpp + + + + + + + + + + + + + + +
LCOV - code coverage report
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Current view:top level - mrs_uav_trackers/src/midair_activation_tracker - midair_activation_tracker.cpp (source / functions)HitTotalCoverage
Test:MRS UAV System - Test coverage reportLines:679272.8 %
Date:2024-04-09 22:03:33Functions: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          82 : 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          82 :   this->common_handlers_  = common_handlers;
+      81          82 :   this->private_handlers_ = private_handlers;
+      82             : 
+      83          82 :   _uav_name_ = common_handlers->uav_name;
+      84             : 
+      85          82 :   nh_ = nh;
+      86             : 
+      87          82 :   ros::Time::waitForValid();
+      88             : 
+      89             :   // --------------------------------------------------------------
+      90             :   // |                     loading parameters                     |
+      91             :   // --------------------------------------------------------------
+      92             : 
+      93             :   // | ---------- loading params using the parent's nh ---------- |
+      94             : 
+      95         164 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+      96             : 
+      97          82 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+      98             : 
+      99          82 :   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          82 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/midair_activation_tracker.yaml");
+     107          82 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/midair_activation_tracker.yaml");
+     108             : 
+     109         164 :   const std::string yaml_prefix = "mrs_uav_trackers/midair_activation_tracker/";
+     110             : 
+     111          82 :   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          82 :   profiler_ = mrs_lib::Profiler(common_handlers->parent_nh, "MidairActivationTracker", _profiler_enabled_);
+     119             : 
+     120             :   // | --------------------- finish the init -------------------- |
+     121             : 
+     122          82 :   is_initialized_ = true;
+     123             : 
+     124          82 :   ROS_INFO("[MidairActivationTracker]: initialized");
+     125             : 
+     126          82 :   return true;
+     127             : }
+     128             : 
+     129             : //}
+     130             : 
+     131             : /* //{ activate() */
+     132             : 
+     133         111 : std::tuple<bool, std::string> MidairActivationTracker::activate([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand> &last_tracker_cmd) {
+     134             : 
+     135         111 :   std::stringstream ss;
+     136             : 
+     137         111 :   is_active_ = true;
+     138             : 
+     139         111 :   ss << "activated";
+     140         111 :   ROS_INFO_STREAM("[MidairActivationTracker]: " << ss.str());
+     141             : 
+     142         222 :   return std::tuple(true, ss.str());
+     143             : }
+     144             : 
+     145             : //}
+     146             : 
+     147             : /* //{ deactivate() */
+     148             : 
+     149         129 : void MidairActivationTracker::deactivate(void) {
+     150             : 
+     151         129 :   is_active_ = false;
+     152             : 
+     153         129 :   ROS_INFO("[MidairActivationTracker]: deactivated");
+     154         129 : }
+     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      111592 : 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      111592 :   if (!is_active_) {
+     174      111355 :     return {};
+     175             :   }
+     176             : 
+     177         711 :   mrs_lib::Routine    profiler_routine = profiler_.createRoutine("update");
+     178             :   mrs_lib::ScopeTimer timer =
+     179         711 :       mrs_lib::ScopeTimer("MidairActivationTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     180             : 
+     181         474 :   mrs_msgs::TrackerCommand tracker_cmd;
+     182             : 
+     183         237 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     184         237 :   tracker_cmd.header.stamp    = ros::Time::now();
+     185             : 
+     186         237 :   tracker_cmd.position.x = uav_state.pose.position.x;
+     187         237 :   tracker_cmd.position.y = uav_state.pose.position.y;
+     188         237 :   tracker_cmd.position.z = uav_state.pose.position.z;
+     189             : 
+     190         237 :   tracker_cmd.velocity.x = uav_state.velocity.linear.x;
+     191         237 :   tracker_cmd.velocity.y = uav_state.velocity.linear.y;
+     192         237 :   tracker_cmd.velocity.z = uav_state.velocity.linear.z;
+     193             : 
+     194             :   try {
+     195         237 :     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         237 :   tracker_cmd.use_position_vertical   = true;
+     203         237 :   tracker_cmd.use_position_horizontal = true;
+     204             : 
+     205         237 :   tracker_cmd.use_velocity_vertical   = true;
+     206         237 :   tracker_cmd.use_velocity_horizontal = true;
+     207             : 
+     208         237 :   tracker_cmd.use_heading = true;
+     209             : 
+     210         237 :   ROS_WARN_THROTTLE(0.1, "[MidairActivationTracker]: outputting cmd");
+     211             : 
+     212         237 :   return {tracker_cmd};
+     213             : }
+     214             : 
+     215             : //}
+     216             : 
+     217             : /* //{ getStatus() */
+     218             : 
+     219          39 : const mrs_msgs::TrackerStatus MidairActivationTracker::getStatus() {
+     220             : 
+     221          39 :   mrs_msgs::TrackerStatus tracker_status;
+     222             : 
+     223          39 :   tracker_status.active            = is_active_;
+     224          39 :   tracker_status.callbacks_enabled = callbacks_enabled_;
+     225             : 
+     226          39 :   return tracker_status;
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* //{ enableCallbacks() */
+     232             : 
+     233         281 : const std_srvs::SetBoolResponse::ConstPtr MidairActivationTracker::enableCallbacks([[maybe_unused]] const std_srvs::SetBoolRequest::ConstPtr &cmd) {
+     234             : 
+     235         562 :   std_srvs::SetBoolResponse res;
+     236             : 
+     237         281 :   res.message = "callbacks are always disabled";
+     238         281 :   res.success = true;
+     239             : 
+     240         562 :   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         255 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MidairActivationTracker::setConstraints([
+     298             :     [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) {
+     299             : 
+     300         510 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+     301             : 
+     302         255 :   res.success = true;
+     303         255 :   res.message = "constraints updated";
+     304             : 
+     305         510 :   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           4 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr MidairActivationTracker::setTrajectoryReference([
+     331             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) {
+     332           4 :   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          82 : 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..e0d5b192d9 --- /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:1320165080.0 %
Date:2024-04-09 22:03:33Functions:405080.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 +
80.0%80.0%
+ 		80.0 %1320 / 165080.0 %40 / 50
<unnamed>80.0 %1320 / 165080.0 %40 / 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..8d850b3f4b --- /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:1320165080.0 %
Date:2024-04-09 22:03:33Functions:405080.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 +
80.0%80.0%
+ 		80.0 %1320 / 165080.0 %40 / 50
<unnamed>80.0 %1320 / 165080.0 %40 / 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..aadd372b36 --- /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:1320165080.0 %
Date:2024-04-09 22:03:33Functions:405080.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 +
80.0%80.0%
+ 		80.0 %1320 / 165080.0 %40 / 50
<unnamed>80.0 %1320 / 165080.0 %40 / 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..b0a618abec --- /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:1320165080.0 %
Date:2024-04-09 22:03:33Functions:405080.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 +
80.0%80.0%
+ 		80.0 %1320 / 165080.0 %40 / 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..ba645ad1d8 --- /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:1320165080.0 %
Date:2024-04-09 22:03:33Functions:405080.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 +
80.0%80.0%
+ 		80.0 %1320 / 165080.0 %40 / 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..91a26effb3 --- /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:1320165080.0 %
Date:2024-04-09 22:03:33Functions:405080.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 +
80.0%80.0%
+ 		80.0 %1320 / 165080.0 %40 / 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..1d4158d802 --- /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:1320165080.0 %
Date:2024-04-09 22:03:33Functions:405080.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::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::resumeTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::stopTrajectoryTrackingImpl[abi:cxx11]()0
mrs_uav_trackers::mpc_tracker::MpcTracker::resumeTrajectoryTrackingImpl[abi:cxx11]()0
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackToggleCollisionAvoidance(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::gotoTrajectoryStart(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::mpc_tracker::MpcTracker::gotoTrajectoryStartImpl[abi:cxx11]()1
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)1
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTrackingImpl[abi:cxx11]()1
mrs_uav_trackers::mpc_tracker::MpcTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)4
mrs_uav_trackers::mpc_tracker::MpcTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)4
mrs_uav_trackers::mpc_tracker::MpcTracker::deactivate()36
mrs_uav_trackers::mpc_tracker::MpcTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)75
(anonymous namespace)::ProxyExec0::ProxyExec0()82
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>)82
mrs_uav_trackers::mpc_tracker::MpcTracker::timerHover(ros::TimerEvent const&)82
mrs_uav_trackers::mpc_tracker::MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig&, unsigned int)82
mrs_uav_trackers::mpc_tracker::MpcTracker::setRelativeGoal(double, double, double, double, bool)157
mrs_uav_trackers::mpc_tracker::MpcTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)244
mrs_uav_trackers::mpc_tracker::MpcTracker::setGoal(double, double, double, double, bool)245
mrs_uav_trackers::mpc_tracker::MpcTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)255
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavTrajectory(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)294
mrs_uav_trackers::mpc_tracker::MpcTracker::setSinglePointReference(double, double, double, double)402
mrs_uav_trackers::mpc_tracker::MpcTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)471
mrs_uav_trackers::mpc_tracker::MpcTracker::timerVelocityTracking(ros::TimerEvent const&)499
mrs_uav_trackers::mpc_tracker::MpcTracker::loadTrajectory[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)501
mrs_uav_trackers::mpc_tracker::MpcTracker::toggleHover(bool)906
mrs_uav_trackers::mpc_tracker::MpcTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)1619
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavDiagnostics(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)1951
mrs_uav_trackers::mpc_tracker::MpcTracker::timerAvoidanceTrajectory(ros::TimerEvent const&)3607
mrs_uav_trackers::mpc_tracker::MpcTracker::getCurrentTrajectoryIdx()6957
mrs_uav_trackers::mpc_tracker::MpcTracker::getStatus()6957
mrs_uav_trackers::mpc_tracker::MpcTracker::increaseCurrentTrajectoryTime(double)12195
mrs_uav_trackers::mpc_tracker::MpcTracker::timerDiagnostics(ros::TimerEvent const&)18196
mrs_uav_trackers::mpc_tracker::MpcTracker::publishDiagnostics()19137
mrs_uav_trackers::mpc_tracker::MpcTracker::iterateModel(double const&)59539
mrs_uav_trackers::mpc_tracker::MpcTracker::checkTrajectoryForCollisions(int&)64312
mrs_uav_trackers::mpc_tracker::MpcTracker::calculateMPC()65275
mrs_uav_trackers::mpc_tracker::MpcTracker::filterReferenceZ(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, double, double)65275
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)65275
mrs_uav_trackers::mpc_tracker::MpcTracker::manageConstraints()65275
mrs_uav_trackers::mpc_tracker::MpcTracker::timerMPC(ros::TimerEvent const&)65275
mrs_uav_trackers::mpc_tracker::MpcTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)111592
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollision(double, double, double, double, double, double)148640
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollisionInflated(double, double, double, double, double, double)148640
+
+
+ + + +
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..a59137d3e5 --- /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:1320165080.0 %
Date:2024-04-09 22:03:33Functions:405080.0 %
Legend: Lines: + hit + not hit +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

Function Name Sort by function nameHit count Sort by hit count
(anonymous namespace)::ProxyExec0::ProxyExec0()82
mrs_uav_trackers::mpc_tracker::MpcTracker::deactivate()36
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>)82
mrs_uav_trackers::mpc_tracker::MpcTracker::timerHover(ros::TimerEvent const&)82
mrs_uav_trackers::mpc_tracker::MpcTracker::resetStatic()0
mrs_uav_trackers::mpc_tracker::MpcTracker::toggleHover(bool)906
mrs_uav_trackers::mpc_tracker::MpcTracker::calculateMPC()65275
mrs_uav_trackers::mpc_tracker::MpcTracker::iterateModel(double const&)59539
mrs_uav_trackers::mpc_tracker::MpcTracker::setReference(boost::shared_ptr<mrs_msgs::ReferenceSrvRequest_<std::allocator<void> > const> const&)244
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)148640
mrs_uav_trackers::mpc_tracker::MpcTracker::loadTrajectory[abi:cxx11](mrs_msgs::TrajectoryReference_<std::allocator<void> >)501
mrs_uav_trackers::mpc_tracker::MpcTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)255
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&)1619
mrs_uav_trackers::mpc_tracker::MpcTracker::setRelativeGoal(double, double, double, double, bool)157
mrs_uav_trackers::mpc_tracker::MpcTracker::filterReferenceZ(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, double, double)65275
mrs_uav_trackers::mpc_tracker::MpcTracker::timerDiagnostics(ros::TimerEvent const&)18196
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)65275
mrs_uav_trackers::mpc_tracker::MpcTracker::manageConstraints()65275
mrs_uav_trackers::mpc_tracker::MpcTracker::publishDiagnostics()19137
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&)1
mrs_uav_trackers::mpc_tracker::MpcTracker::setVelocityReference(boost::shared_ptr<mrs_msgs::VelocityReferenceSrvRequest_<std::allocator<void> > const> const&)471
mrs_uav_trackers::mpc_tracker::MpcTracker::switchOdometrySource(mrs_msgs::UavState_<std::allocator<void> > const&)4
mrs_uav_trackers::mpc_tracker::MpcTracker::timerVelocityTracking(ros::TimerEvent const&)499
mrs_uav_trackers::mpc_tracker::MpcTracker::checkCollisionInflated(double, double, double, double, double, double)148640
mrs_uav_trackers::mpc_tracker::MpcTracker::setTrajectoryReference(boost::shared_ptr<mrs_msgs::TrajectoryReferenceSrvRequest_<std::allocator<void> > const> const&)4
mrs_uav_trackers::mpc_tracker::MpcTracker::stopTrajectoryTracking(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::getCurrentTrajectoryIdx()6957
mrs_uav_trackers::mpc_tracker::MpcTracker::gotoTrajectoryStartImpl[abi:cxx11]()1
mrs_uav_trackers::mpc_tracker::MpcTracker::setSinglePointReference(double, double, double, double)402
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTracking(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&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::timerAvoidanceTrajectory(ros::TimerEvent const&)3607
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavTrajectory(boost::shared_ptr<mrs_msgs::FutureTrajectory_<std::allocator<void> > const>)294
mrs_uav_trackers::mpc_tracker::MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig&, unsigned int)82
mrs_uav_trackers::mpc_tracker::MpcTracker::stopTrajectoryTrackingImpl[abi:cxx11]()0
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackOtherMavDiagnostics(boost::shared_ptr<mrs_msgs::MpcTrackerDiagnostics_<std::allocator<void> > const>)1951
mrs_uav_trackers::mpc_tracker::MpcTracker::startTrajectoryTrackingImpl[abi:cxx11]()1
mrs_uav_trackers::mpc_tracker::MpcTracker::checkTrajectoryForCollisions(int&)64312
mrs_uav_trackers::mpc_tracker::MpcTracker::resumeTrajectoryTrackingImpl[abi:cxx11]()0
mrs_uav_trackers::mpc_tracker::MpcTracker::increaseCurrentTrajectoryTime(double)12195
mrs_uav_trackers::mpc_tracker::MpcTracker::callbackToggleCollisionAvoidance(std_srvs::SetBoolRequest_<std::allocator<void> >&, std_srvs::SetBoolResponse_<std::allocator<void> >&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::hover(boost::shared_ptr<std_srvs::TriggerRequest_<std::allocator<void> > const> const&)0
mrs_uav_trackers::mpc_tracker::MpcTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)111592
mrs_uav_trackers::mpc_tracker::MpcTracker::setGoal(double, double, double, double, bool)245
mrs_uav_trackers::mpc_tracker::MpcTracker::activate[abi:cxx11](std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&)75
mrs_uav_trackers::mpc_tracker::MpcTracker::timerMPC(ros::TimerEvent const&)65275
mrs_uav_trackers::mpc_tracker::MpcTracker::getStatus()6957
+
+
+ + + +
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..c53b882e89 --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.html @@ -0,0 +1,3931 @@ + + + + + + + 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:1320165080.0 %
Date:2024-04-09 22:03:33Functions:405080.0 %
Legend: Lines: + hit + not hit +
+
+ + + + + + + + +

+
          Line data    Source code
+
+       1             : /* includes //{ */
+       2             : 
+       3             : #include <ros/ros.h>
+       4             : #include <ros/package.h>
+       5             : 
+       6             : #include <mrs_uav_managers/tracker.h>
+       7             : #include <mrs_uav_managers/controller.h>
+       8             : 
+       9             : #include <geometry_msgs/Pose.h>
+      10             : #include <geometry_msgs/PoseArray.h>
+      11             : 
+      12             : #include <mrs_msgs/FuturePoint.h>
+      13             : #include <mrs_msgs/FutureTrajectory.h>
+      14             : #include <mrs_msgs/MpcTrackerDiagnostics.h>
+      15             : #include <mrs_msgs/MpcPredictionFullState.h>
+      16             : #include <mrs_msgs/EstimationDiagnostics.h>
+      17             : #include <mrs_msgs/VelocityReference.h>
+      18             : #include <mrs_msgs/VelocityReferenceSrv.h>
+      19             : 
+      20             : #include <std_msgs/String.h>
+      21             : 
+      22             : #include <mrs_lib/profiler.h>
+      23             : #include <mrs_lib/utils.h>
+      24             : #include <mrs_lib/mutex.h>
+      25             : #include <mrs_lib/attitude_converter.h>
+      26             : #include <mrs_lib/subscribe_handler.h>
+      27             : #include <mrs_lib/publisher_handler.h>
+      28             : #include <mrs_lib/geometry/cyclic.h>
+      29             : #include <mrs_lib/geometry/misc.h>
+      30             : #include <mrs_lib/scope_timer.h>
+      31             : 
+      32             : #include <mpc_tracker.h>
+      33             : 
+      34             : #include <dynamic_reconfigure/server.h>
+      35             : #include <mrs_uav_trackers/mpc_trackerConfig.h>
+      36             : 
+      37             : #include <visualization_msgs/Marker.h>
+      38             : #include <visualization_msgs/MarkerArray.h>
+      39             : 
+      40             : //}
+      41             : 
+      42             : /* defines //{ */
+      43             : 
+      44             : #define MPC_N_STATES 12
+      45             : #define MPC_N_INPUTS 3
+      46             : 
+      47             : #define MPC_HEADING_N_STATES 4
+      48             : #define MPC_HEADING_N_INPUTS 1
+      49             : 
+      50             : #define MPC_HORIZON_LENGTH 40
+      51             : 
+      52             : //}
+      53             : 
+      54             : /* using //{ */
+      55             : 
+      56             : using namespace Eigen;
+      57             : 
+      58             : using vec2_t = mrs_lib::geometry::vec_t<2>;
+      59             : using vec3_t = mrs_lib::geometry::vec_t<3>;
+      60             : 
+      61             : using radians  = mrs_lib::geometry::radians;
+      62             : using sradians = mrs_lib::geometry::sradians;
+      63             : 
+      64             : using quat_t = Eigen::Quaterniond;
+      65             : 
+      66             : //}
+      67             : 
+      68             : namespace mrs_uav_trackers
+      69             : {
+      70             : 
+      71             : namespace mpc_tracker
+      72             : {
+      73             : 
+      74             : /* //{ class MpcTracker */
+      75             : 
+      76             : class MpcTracker : public mrs_uav_managers::Tracker {
+      77             : public:
+      78             :   bool initialize(const ros::NodeHandle& nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+      79             :                   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers);
+      80             : 
+      81             :   std::tuple<bool, std::string> activate(const std::optional<mrs_msgs::TrackerCommand>& last_tracker_cmd);
+      82             :   void                          deactivate(void);
+      83             :   bool                          resetStatic(void);
+      84             : 
+      85             :   std::optional<mrs_msgs::TrackerCommand>   update(const mrs_msgs::UavState& uav_state, const mrs_uav_managers::Controller::ControlOutput& last_control_output);
+      86             :   const std_srvs::SetBoolResponse::ConstPtr enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr& cmd);
+      87             :   const mrs_msgs::TrackerStatus             getStatus();
+      88             :   const std_srvs::TriggerResponse::ConstPtr switchOdometrySource(const mrs_msgs::UavState& new_uav_state);
+      89             : 
+      90             :   const mrs_msgs::ReferenceSrvResponse::ConstPtr           setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr& cmd);
+      91             :   const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr   setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr& cmd);
+      92             :   const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr setTrajectoryReference(const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr& cmd);
+      93             : 
+      94             :   const std_srvs::TriggerResponse::ConstPtr hover(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      95             :   const std_srvs::TriggerResponse::ConstPtr startTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      96             :   const std_srvs::TriggerResponse::ConstPtr stopTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      97             :   const std_srvs::TriggerResponse::ConstPtr resumeTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      98             :   const std_srvs::TriggerResponse::ConstPtr gotoTrajectoryStart(const std_srvs::TriggerRequest::ConstPtr& cmd);
+      99             : 
+     100             :   const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& cmd);
+     101             : 
+     102             : private:
+     103             :   ros::NodeHandle nh_;
+     104             : 
+     105             :   std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t>  common_handlers_;
+     106             :   std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers_;
+     107             : 
+     108             :   std::atomic<bool> callbacks_enabled_ = true;
+     109             : 
+     110             :   std::string _uav_name_;
+     111             : 
+     112             :   // debugging publishers
+     113             :   mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics> pub_diagnostics_;
+     114             :   mrs_lib::PublisherHandler<std_msgs::String>                pub_status_string_;
+     115             : 
+     116             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>        pub_debug_processed_trajectory_poses_;
+     117             :   mrs_lib::PublisherHandler<visualization_msgs::MarkerArray> pub_debug_processed_trajectory_markers_;
+     118             : 
+     119             :   mrs_msgs::UavState uav_state_;
+     120             :   std::mutex         mutex_uav_state_;
+     121             : 
+     122             :   bool is_active_      = false;
+     123             :   bool is_initialized_ = false;
+     124             : 
+     125             :   // | ----------------------- constraints ---------------------- |
+     126             : 
+     127             :   mrs_msgs::DynamicsConstraints constraints_;
+     128             :   std::mutex                    mutex_constraints_;
+     129             : 
+     130             :   mrs_msgs::DynamicsConstraints constraints_filtered_;
+     131             :   std::mutex                    mutex_constraints_filtered_;
+     132             : 
+     133             :   std::atomic<bool> got_constraints_     = false;
+     134             :   std::atomic<bool> all_constraints_set_ = false;
+     135             : 
+     136             :   double _diag_pos_tracking_thr_;
+     137             :   double _diag_heading_tracking_thr_;
+     138             : 
+     139             :   double _mpc_synchronous_rate_limit_;
+     140             :   double _mpc_asynchronous_rate_;
+     141             : 
+     142             :   double update_rate_ = 100.0;
+     143             : 
+     144             :   double     dt1_;
+     145             :   std::mutex mutex_dt1_;
+     146             : 
+     147             :   double _dt2_;
+     148             : 
+     149             :   MatrixXd          _mat_A_;  // system matrix for virtual UAV
+     150             :   MatrixXd          _mat_B_;  // input matrix for virtual UAV
+     151             :   MatrixXd          A_;       // system matrix for virtual UAV
+     152             :   MatrixXd          B_;       // input matrix for virtual UAV
+     153             :   std::atomic<bool> model_first_iteration_ = true;
+     154             :   ros::Time         model_iteration_last_time_;
+     155             : 
+     156             :   MatrixXd _mat_A_heading_;  // system matrix for heading
+     157             :   MatrixXd _mat_B_heading_;  // input matrix for heading
+     158             :   MatrixXd A_heading_;       // system matrix for heading
+     159             :   MatrixXd B_heading_;       // input matrix for heading
+     160             : 
+     161             :   // the reference over the prediction horizon per axis
+     162             :   MatrixXd   des_x_trajectory_;
+     163             :   MatrixXd   des_y_trajectory_;
+     164             :   MatrixXd   des_z_trajectory_;
+     165             :   MatrixXd   des_heading_trajectory_;
+     166             :   std::mutex mutex_des_trajectory_;
+     167             : 
+     168             :   // the reference filtered over the prediction horizon per axis
+     169             :   MatrixXd des_z_filtered_offset_;
+     170             : 
+     171             :   // the whole trajectory reference split per axis
+     172             :   std::shared_ptr<VectorXd> des_x_whole_trajectory_;
+     173             :   std::shared_ptr<VectorXd> des_y_whole_trajectory_;
+     174             :   std::shared_ptr<VectorXd> des_z_whole_trajectory_;
+     175             :   std::shared_ptr<VectorXd> des_heading_whole_trajectory_;
+     176             :   int                       des_whole_trajectory_id_ = 0;
+     177             :   std::mutex                mutex_des_whole_trajectory_;
+     178             : 
+     179             :   int  getCurrentTrajectoryIdx();
+     180             :   void increaseCurrentTrajectoryTime(const double dt);
+     181             : 
+     182             :   // trajectory tracking
+     183             :   std::atomic<bool> trajectory_tracking_in_progress_ = false;
+     184             :   double            trajectory_current_time_;
+     185             :   std::mutex        mutex_trajectory_tracking_states_;
+     186             : 
+     187             :   // params of the loaded trajectory
+     188             :   int    trajectory_size_          = 0;
+     189             :   double trajectory_dt_            = 0.2;
+     190             :   bool   trajectory_track_heading_ = false;
+     191             :   bool   trajectory_tracking_loop_ = false;
+     192             :   bool   trajectory_set_           = false;
+     193             :   int    trajectory_count_         = 0;  // counts how many trajectories we have received
+     194             : 
+     195             :   // mpc output
+     196             :   VectorXd   mpc_u_;
+     197             :   double     mpc_u_heading_;
+     198             :   std::mutex mutex_mpc_u_;
+     199             : 
+     200             :   // current state of the dynamical system
+     201             :   MatrixXd   mpc_x_;          // translation state
+     202             :   MatrixXd   mpc_x_heading_;  // heading state
+     203             :   std::mutex mutex_mpc_x_;
+     204             : 
+     205             :   // odometry reset
+     206             :   std::atomic<bool> odometry_reset_in_progress_ = false;
+     207             :   std::atomic<bool> mpc_result_invalid_         = false;
+     208             : 
+     209             :   // predicting the future
+     210             :   MatrixXd   predicted_trajectory_;
+     211             :   MatrixXd   predicted_heading_trajectory_;
+     212             :   std::mutex mutex_predicted_trajectory_;
+     213             : 
+     214             :   mrs_msgs::MpcPredictionFullState prediction_full_state_;
+     215             :   std::mutex                       mutex_prediction_full_state_;
+     216             : 
+     217             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>   ph_predicted_trajectory_debugging_;
+     218             :   mrs_lib::PublisherHandler<geometry_msgs::PoseArray>   ph_mpc_reference_debugging_;
+     219             :   mrs_lib::PublisherHandler<geometry_msgs::PoseStamped> ph_current_trajectory_point_;
+     220             : 
+     221             :   std::atomic<bool> mpc_computed_ = false;
+     222             : 
+     223             :   bool brake_ = false;
+     224             : 
+     225             :   // | ----------------------- MPC solver ----------------------- |
+     226             : 
+     227             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_y_;
+     228             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_x_;
+     229             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_z_;
+     230             :   std::shared_ptr<mrs_mpc_solvers::mpc_tracker::Solver> mpc_solver_heading_;
+     231             : 
+     232             :   std::mutex mutex_mpc_calculation_;
+     233             : 
+     234             :   int _max_iters_xy_;
+     235             :   int _max_iters_z_;
+     236             :   int _max_iters_heading_;
+     237             : 
+     238             :   // | ----------- measuring the "MPC realtime factor" ---------- |
+     239             : 
+     240             :   double mpc_rtf_ = 0.0;
+     241             : 
+     242             :   // | ------------------- collision avoidance ------------------ |
+     243             : 
+     244             :   // configurable params
+     245             :   bool collision_avoidance_enabled_           = false;
+     246             :   bool collision_avoidance_enabled_passively_ = true;
+     247             : 
+     248             :   // TODO what is this?
+     249             :   double    coef_scaler = 0;
+     250             :   ros::Time coef_time;
+     251             : 
+     252             :   double minimum_collison_free_altitude_ = std::numeric_limits<double>::lowest();
+     253             : 
+     254             :   // params
+     255             :   double                   _avoidance_trajectory_rate_;
+     256             :   double                   _avoidance_radius_threshold_;
+     257             :   double                   _avoidance_z_correction_;
+     258             :   std::string              _avoidance_diagnostics_topic_name_;
+     259             :   std::vector<std::string> _avoidance_other_uav_names_;
+     260             :   double                   _avoidance_z_threshold_;
+     261             : 
+     262             :   // how old can the other UAV trajectory be (since receive time)
+     263             :   double _collision_trajectory_timeout_;
+     264             : 
+     265             :   // when collision detected, slow down during the manouver
+     266             :   double _avoidance_collision_horizontal_speed_coef_;
+     267             : 
+     268             :   // when collision detected, slow down fully this number of steps before it
+     269             :   int _avoidance_collision_slow_down_fully_;
+     270             : 
+     271             :   // when collision detected, start slowing down this number of steps before it
+     272             :   int _avoidance_collision_slow_down_;
+     273             : 
+     274             :   // when avoiding, start climbing this number of steps before it
+     275             :   int _avoidance_collision_start_climbing_;
+     276             : 
+     277             :   int avoidance_this_uav_number_;
+     278             :   int avoidance_this_uav_priority_;
+     279             : 
+     280             :   double            collision_free_altitude_;
+     281             :   std::atomic<bool> avoiding_collision_               = false;
+     282             :   bool              collision_avoidance_affecting_me_ = false;
+     283             : 
+     284             :   // avoidance trajectory will not be published unless we computed it at least once
+     285             :   std::atomic<bool> future_was_predicted_ = false;
+     286             : 
+     287             :   // subscribing to the other UAV future trajectories
+     288             :   void callbackOtherMavTrajectory(const mrs_msgs::FutureTrajectory::ConstPtr msg);
+     289             : 
+     290             :   std::vector<mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory>> other_uav_trajectory_subscribers_;
+     291             :   std::map<std::string, mrs_msgs::FutureTrajectory>                  other_uav_avoidance_trajectories_;
+     292             :   std::mutex                                                         mutex_other_uav_avoidance_trajectories_;
+     293             : 
+     294             :   // subscribing to the other UAV diagnostics'
+     295             :   void callbackOtherMavDiagnostics(const mrs_msgs::MpcTrackerDiagnostics::ConstPtr msg);
+     296             : 
+     297             :   std::vector<mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics>> other_uav_diag_subscribers_;
+     298             :   std::map<std::string, mrs_msgs::MpcTrackerDiagnostics>                  other_uav_diagnostics_;
+     299             :   std::mutex                                                              mutex_other_uav_diagnostics_;
+     300             : 
+     301             :   bool checkCollision(const double ax, const double ay, const double az, const double bx, const double by, const double bz);
+     302             :   bool checkCollisionInflated(const double ax, const double ay, const double az, const double bx, const double by, const double bz);
+     303             : 
+     304             :   mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory> ph_avoidance_trajectory_;
+     305             : 
+     306             :   ros::ServiceServer service_server_toggle_avoidance_;
+     307             :   bool               callbackToggleCollisionAvoidance(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     308             : 
+     309             :   mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics> sh_estimation_diag_;
+     310             : 
+     311             :   // | --------------------- MPC calculation -------------------- |
+     312             : 
+     313             :   ros::Timer        timer_mpc_iteration_;
+     314             :   std::atomic<bool> mpc_synchronous_ = false;
+     315             :   ros::TimerEvent   synchronous_timer_event_;
+     316             : 
+     317             :   std::atomic<bool> mpc_timer_running_ = false;
+     318             :   void              timerMPC(const ros::TimerEvent& event);
+     319             : 
+     320             :   // | -------------------- velocity tracking ------------------- |
+     321             : 
+     322             :   ros::Timer                  timer_velocity_tracking_;
+     323             :   void                        timerVelocityTracking(const ros::TimerEvent& event);
+     324             :   ros::Time                   velocity_reference_time_;
+     325             :   mrs_msgs::VelocityReference velocity_reference_;
+     326             :   std::mutex                  mutex_velocity_reference_;
+     327             :   std::atomic<bool>           velocity_tracking_active_ = false;
+     328             : 
+     329             :   // | ------------------ avoidance trajectory ------------------ |
+     330             : 
+     331             :   ros::Timer timer_avoidance_trajectory_;
+     332             :   void       timerAvoidanceTrajectory(const ros::TimerEvent& event);
+     333             : 
+     334             :   // | ----------------------- diagnostics ---------------------- |
+     335             : 
+     336             :   ros::Timer timer_diagnostics_;
+     337             :   double     _diagnostics_rate_;
+     338             :   void       timerDiagnostics(const ros::TimerEvent& event);
+     339             : 
+     340             :   // | ------------------------ hovering ------------------------ |
+     341             : 
+     342             :   ros::Timer        timer_hover_;
+     343             :   void              timerHover(const ros::TimerEvent& event);
+     344             :   std::atomic<bool> hovering_in_progress_ = false;
+     345             :   void              toggleHover(bool in);
+     346             : 
+     347             :   // | ------------------- trajectory tracking ------------------ |
+     348             : 
+     349             :   std::tuple<bool, std::string> resumeTrajectoryTrackingImpl(void);
+     350             :   std::tuple<bool, std::string> startTrajectoryTrackingImpl(void);
+     351             :   std::tuple<bool, std::string> stopTrajectoryTrackingImpl(void);
+     352             :   std::tuple<bool, std::string> gotoTrajectoryStartImpl(void);
+     353             : 
+     354             :   // | --------------------- other routines --------------------- |
+     355             : 
+     356             :   void publishDiagnostics();
+     357             : 
+     358             :   void debugPrintState(const double throttle);
+     359             :   void debugPrintMPCResult(const double throttle);
+     360             : 
+     361             :   void setGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading);
+     362             :   void setRelativeGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading);
+     363             :   void setSinglePointReference(const double x, const double y, const double z, const double heading);
+     364             : 
+     365             :   std::tuple<bool, std::string, bool> loadTrajectory(const mrs_msgs::TrajectoryReference msg);
+     366             : 
+     367             :   MatrixXd                       filterReferenceZ(const VectorXd& des_z_trajectory, const double max_ascending_speed, const double max_descending_speed);
+     368             :   std::tuple<MatrixXd, MatrixXd> filterReferenceXY(const VectorXd& des_x_trajectory, const VectorXd& des_y_trajectory, double max_speed_x, double max_speed_y);
+     369             : 
+     370             :   double checkTrajectoryForCollisions(int& first_collision_index);
+     371             : 
+     372             :   void manageConstraints(void);
+     373             :   void calculateMPC(void);
+     374             :   void iterateModel(const double& dt);
+     375             : 
+     376             :   // | ------------------------ profiler ------------------------ |
+     377             : 
+     378             :   mrs_lib::Profiler profiler;
+     379             :   bool              _profiler_enabled_ = false;
+     380             : 
+     381             :   // | ------------------------- wiggle ------------------------- |
+     382             : 
+     383             :   ros::ServiceServer service_server_wiggle_;
+     384             :   bool               callbackWiggle(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+     385             : 
+     386             :   double wiggle_phase_ = 0;
+     387             : 
+     388             :   // | --------------- dynamic reconfigure server --------------- |
+     389             : 
+     390             :   void dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig& config, uint32_t level);
+     391             : 
+     392             :   boost::recursive_mutex                      config_mutex_;
+     393             :   typedef mrs_uav_trackers::mpc_trackerConfig Config;
+     394             :   typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
+     395             :   boost::shared_ptr<ReconfigureServer>        reconfigure_server_;
+     396             :   mrs_uav_trackers::mpc_trackerConfig         drs_params_;
+     397             :   std::mutex                                  mutex_drs_params_;
+     398             : };
+     399             : 
+     400             : //}
+     401             : 
+     402             : // | -------------- tracker's interface routines -------------- |
+     403             : 
+     404             : /* //{ initialize() */
+     405             : 
+     406          82 : bool MpcTracker::initialize(const ros::NodeHandle& nh, std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers,
+     407             :                             std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers) {
+     408             : 
+     409          82 :   nh_ = nh;
+     410             : 
+     411          82 :   common_handlers_  = common_handlers;
+     412          82 :   private_handlers_ = private_handlers;
+     413             : 
+     414          82 :   _uav_name_ = common_handlers->uav_name;
+     415             : 
+     416          82 :   ros::Time::waitForValid();
+     417             : 
+     418             :   // --------------------------------------------------------------
+     419             :   // |                     loading parameters                     |
+     420             :   // --------------------------------------------------------------
+     421             : 
+     422             :   // | ---------- loading params using the parent's nh ---------- |
+     423             : 
+     424         164 :   mrs_lib::ParamLoader param_loader_parent(common_handlers->parent_nh, "ControlManager");
+     425             : 
+     426          82 :   param_loader_parent.loadParam("enable_profiler", _profiler_enabled_);
+     427             : 
+     428          82 :   if (!param_loader_parent.loadedSuccessfully()) {
+     429           0 :     ROS_ERROR("[MpcTracker]: Could not load all parameters!");
+     430           0 :     return false;
+     431             :   }
+     432             : 
+     433             :   // | --------------- loading plugin's parameters -------------- |
+     434             : 
+     435          82 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/private/mpc_tracker.yaml");
+     436          82 :   private_handlers->param_loader->addYamlFile(ros::package::getPath("mrs_uav_trackers") + "/config/public/mpc_tracker.yaml");
+     437             : 
+     438         164 :   const std::string yaml_prefix = "mrs_uav_trackers/mpc_tracker/";
+     439             : 
+     440          82 :   private_handlers->param_loader->loadParam("network/robot_names", _avoidance_other_uav_names_);
+     441             : 
+     442          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_loop/synchronous_rate_limit", _mpc_synchronous_rate_limit_);
+     443          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_loop/asynchronous_loop_rate", _mpc_asynchronous_rate_);
+     444             : 
+     445          82 :   if (_mpc_asynchronous_rate_ < 15) {
+     446           0 :     ROS_ERROR("[MpcTracker]: the asynchronous_loop_rate must be > 15 Hz");
+     447           0 :     return false;
+     448             :   }
+     449             : 
+     450          82 :   dt1_ = 1.0 / _mpc_asynchronous_rate_;
+     451             : 
+     452          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/enabled", drs_params_.braking_enabled);
+     453          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/q_vel_braking", drs_params_.q_vel_braking);
+     454          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "braking/q_vel_no_braking", drs_params_.q_vel_no_braking);
+     455             : 
+     456          82 :   private_handlers->param_loader->loadMatrixKnown(yaml_prefix + "model/translation/A", _mat_A_, MPC_N_STATES, MPC_N_STATES);
+     457          82 :   private_handlers->param_loader->loadMatrixKnown(yaml_prefix + "model/translation/B", _mat_B_, MPC_N_STATES, MPC_N_INPUTS);
+     458             : 
+     459          82 :   A_ = _mat_A_;
+     460          82 :   B_ = _mat_B_;
+     461             : 
+     462          82 :   private_handlers->param_loader->loadMatrixKnown(yaml_prefix + "model/heading/A", _mat_A_heading_, MPC_HEADING_N_STATES, MPC_HEADING_N_STATES);
+     463          82 :   private_handlers->param_loader->loadMatrixKnown(yaml_prefix + "model/heading/B", _mat_B_heading_, MPC_HEADING_N_STATES, MPC_HEADING_N_INPUTS);
+     464             : 
+     465          82 :   A_heading_ = _mat_A_heading_;
+     466          82 :   B_heading_ = _mat_B_heading_;
+     467             : 
+     468             :   // load the MPC parameters
+     469          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/dt2", _dt2_);
+     470             : 
+     471          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/rate", _diagnostics_rate_);
+     472          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/position_tracking_threshold", _diag_pos_tracking_thr_);
+     473          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "diagnostics/orientation_tracking_threshold", _diag_heading_tracking_thr_);
+     474             : 
+     475          82 :   bool verbose_xy      = false;
+     476          82 :   bool verbose_z       = false;
+     477          82 :   bool verbose_heading = false;
+     478             : 
+     479         164 :   std::vector<double> xy_Q;
+     480         164 :   std::vector<double> z_Q;
+     481         164 :   std::vector<double> heading_Q;
+     482             : 
+     483          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/verbose", verbose_xy);
+     484          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/max_n_iterations", _max_iters_xy_);
+     485          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/xy/Q", xy_Q);
+     486             : 
+     487          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/verbose", verbose_z);
+     488          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/max_n_iterations", _max_iters_z_);
+     489          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/z/Q", z_Q);
+     490             : 
+     491          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/verbose", verbose_heading);
+     492          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/max_n_iterations", _max_iters_heading_);
+     493          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "mpc_solver/heading/Q", heading_Q);
+     494             : 
+     495          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/enabled", drs_params_.wiggle_enabled);
+     496          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/amplitude", drs_params_.wiggle_amplitude);
+     497          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "wiggle/frequency", drs_params_.wiggle_frequency);
+     498             : 
+     499          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/enabled", collision_avoidance_enabled_);
+     500          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/enabled_passively", collision_avoidance_enabled_passively_);
+     501          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/predicted_trajectory_publish_rate", _avoidance_trajectory_rate_);
+     502          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/correction", _avoidance_z_correction_);
+     503          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/radius", _avoidance_radius_threshold_);
+     504          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/altitude_threshold", _avoidance_z_threshold_);
+     505          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_horizontal_speed_coef", _avoidance_collision_horizontal_speed_coef_);
+     506          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_slow_down_fully", _avoidance_collision_slow_down_fully_);
+     507          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_slow_down_start", _avoidance_collision_slow_down_);
+     508          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/collision_start_climbing", _avoidance_collision_start_climbing_);
+     509          82 :   private_handlers->param_loader->loadParam(yaml_prefix + "collision_avoidance/trajectory_timeout", _collision_trajectory_timeout_);
+     510             : 
+     511          82 :   if (!private_handlers->param_loader->loadedSuccessfully()) {
+     512           0 :     ROS_ERROR("[MpcTracker]: could not load all parameters!");
+     513           0 :     return false;
+     514             :   }
+     515             : 
+     516          82 :   ROS_INFO_STREAM("[MpcTracker]: initializing solvers with dt1 = " << dt1_);
+     517             : 
+     518          82 :   mpc_solver_y_ = std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_y", verbose_xy, _max_iters_xy_, xy_Q, dt1_, _dt2_, 1);
+     519          82 :   mpc_solver_x_ = std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_x", verbose_xy, _max_iters_xy_, xy_Q, dt1_, _dt2_, 0);
+     520          82 :   mpc_solver_z_ = std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_z", verbose_z, _max_iters_z_, z_Q, dt1_, _dt2_, 2);
+     521             :   mpc_solver_heading_ =
+     522          82 :       std::make_shared<mrs_mpc_solvers::mpc_tracker::Solver>("MpcTracker_hdg", verbose_heading, _max_iters_heading_, heading_Q, dt1_, _dt2_, 0);
+     523             : 
+     524          82 :   mpc_x_         = MatrixXd::Zero(MPC_N_STATES, 1);
+     525          82 :   mpc_x_heading_ = MatrixXd::Zero(MPC_HEADING_N_STATES, 1);
+     526             : 
+     527          82 :   mpc_u_ = VectorXd::Zero(MPC_N_INPUTS);
+     528             : 
+     529          82 :   coef_time = ros::Time(0);
+     530             : 
+     531          82 :   des_x_trajectory_       = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     532          82 :   des_y_trajectory_       = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     533          82 :   des_z_trajectory_       = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     534          82 :   des_z_filtered_offset_  = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     535          82 :   des_heading_trajectory_ = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+     536             : 
+     537          82 :   service_server_wiggle_ = nh_.advertiseService("wiggle", &MpcTracker::callbackWiggle, this);
+     538             : 
+     539          82 :   pub_diagnostics_   = mrs_lib::PublisherHandler<mrs_msgs::MpcTrackerDiagnostics>(nh_, "diagnostics", 1);
+     540          82 :   pub_status_string_ = mrs_lib::PublisherHandler<std_msgs::String>(nh_, "string", 1);
+     541             : 
+     542             :   // extract the numerical name
+     543          82 :   sscanf(_uav_name_.c_str(), "uav%d", &avoidance_this_uav_number_);
+     544          82 :   ROS_INFO("[MpcTracker]: Numerical ID of this UAV is %d", avoidance_this_uav_number_);
+     545          82 :   avoidance_this_uav_priority_ = avoidance_this_uav_number_;
+     546             : 
+     547             :   // exclude this drone from the list
+     548          82 :   std::vector<std::string>::iterator it = _avoidance_other_uav_names_.begin();
+     549         174 :   while (it != _avoidance_other_uav_names_.end()) {
+     550             : 
+     551          92 :     std::string temp_str = *it;
+     552             : 
+     553             :     int other_uav_priority;
+     554          92 :     sscanf(temp_str.c_str(), "uav%d", &other_uav_priority);
+     555             : 
+     556          92 :     if (other_uav_priority == avoidance_this_uav_number_) {
+     557             : 
+     558          82 :       _avoidance_other_uav_names_.erase(it);
+     559          82 :       continue;
+     560             :     }
+     561             : 
+     562          10 :     it++;
+     563             :   }
+     564             : 
+     565             :   // initialize velocity tracker
+     566             : 
+     567          82 :   velocity_reference_time_ = ros::Time(0);
+     568             : 
+     569             :   // create publishers for predicted trajectory
+     570             : 
+     571          82 :   ph_avoidance_trajectory_           = mrs_lib::PublisherHandler<mrs_msgs::FutureTrajectory>(nh_, "predicted_trajectory", 1);
+     572          82 :   ph_predicted_trajectory_debugging_ = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "predicted_trajectory_debugging", 1);
+     573          82 :   ph_mpc_reference_debugging_        = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "mpc_reference_debugging", 1, true);
+     574          82 :   ph_current_trajectory_point_       = mrs_lib::PublisherHandler<geometry_msgs::PoseStamped>(nh_, "current_trajectory_point", 1, true);
+     575             : 
+     576          82 :   pub_debug_processed_trajectory_poses_   = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "trajectory_processed/poses", 1, true);
+     577          82 :   pub_debug_processed_trajectory_markers_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "trajectory_processed/markers", 1, true);
+     578             : 
+     579             :   // preallocate predicted trajectory
+     580          82 :   predicted_trajectory_         = MatrixXd::Zero(MPC_HORIZON_LENGTH * MPC_N_STATES, 1);
+     581          82 :   predicted_heading_trajectory_ = MatrixXd::Zero(MPC_HORIZON_LENGTH * MPC_N_STATES, 1);
+     582             : 
+     583          82 :   collision_free_altitude_ = std::numeric_limits<float>::lowest();
+     584             : 
+     585             :   // collision avoidance toggle service
+     586          82 :   service_server_toggle_avoidance_ = nh_.advertiseService("collision_avoidance", &MpcTracker::callbackToggleCollisionAvoidance, this);
+     587             : 
+     588         164 :   mrs_lib::SubscribeHandlerOptions shopts;
+     589          82 :   shopts.nh                 = nh_;
+     590          82 :   shopts.node_name          = "MpcTracker";
+     591          82 :   shopts.no_message_timeout = mrs_lib::no_timeout;
+     592          82 :   shopts.threadsafe         = true;
+     593          82 :   shopts.autostart          = true;
+     594          82 :   shopts.queue_size         = 10;
+     595          82 :   shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+     596             : 
+     597             :   // create subscribers on other drones diagnostics
+     598          82 :   if (collision_avoidance_enabled_ || collision_avoidance_enabled_passively_) {
+     599             : 
+     600          92 :     for (int i = 0; i < int(_avoidance_other_uav_names_.size()); i++) {
+     601             : 
+     602          30 :       std::string prediction_topic_name = std::string("/") + _avoidance_other_uav_names_[i] + "/control_manager/mpc_tracker/predicted_trajectory";
+     603          20 :       std::string diag_topic_name       = std::string("/") + _avoidance_other_uav_names_[i] + "/control_manager/mpc_tracker/diagnostics";
+     604             : 
+     605          10 :       ROS_INFO("[MpcTracker]: subscribing to %s", prediction_topic_name.c_str());
+     606             : 
+     607          10 :       other_uav_trajectory_subscribers_.push_back(
+     608          20 :           mrs_lib::SubscribeHandler<mrs_msgs::FutureTrajectory>(shopts, prediction_topic_name, &MpcTracker::callbackOtherMavTrajectory, this));
+     609             : 
+     610          10 :       ROS_INFO("[MpcTracker]: subscribing to %s", diag_topic_name.c_str());
+     611             : 
+     612          10 :       other_uav_diag_subscribers_.push_back(
+     613          20 :           mrs_lib::SubscribeHandler<mrs_msgs::MpcTrackerDiagnostics>(shopts, diag_topic_name, &MpcTracker::callbackOtherMavDiagnostics, this));
+     614             :     }
+     615             :   }
+     616             : 
+     617          82 :   sh_estimation_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, std::string("/") + _uav_name_ + "/estimation_manager/diagnostics");
+     618             : 
+     619             :   // | --------------- dynamic reconfigure server --------------- |
+     620             : 
+     621          82 :   reconfigure_server_.reset(new ReconfigureServer(config_mutex_, nh_));
+     622          82 :   reconfigure_server_->updateConfig(drs_params_);
+     623          82 :   ReconfigureServer::CallbackType f = boost::bind(&MpcTracker::dynamicReconfigureCallback, this, _1, _2);
+     624          82 :   reconfigure_server_->setCallback(f);
+     625             : 
+     626             :   // | ------------------------ profiler ------------------------ |
+     627             : 
+     628          82 :   profiler = mrs_lib::Profiler(common_handlers->parent_nh, "MpcTracker", _profiler_enabled_);
+     629             : 
+     630             :   // | ------------------------- timers ------------------------- |
+     631             : 
+     632         164 :   timer_avoidance_trajectory_ = nh_.createTimer(ros::Rate(_avoidance_trajectory_rate_), &MpcTracker::timerAvoidanceTrajectory, this, false,
+     633         164 :                                                 collision_avoidance_enabled_ || collision_avoidance_enabled_passively_);
+     634          82 :   timer_diagnostics_          = nh_.createTimer(ros::Rate(_diagnostics_rate_), &MpcTracker::timerDiagnostics, this);
+     635          82 :   timer_mpc_iteration_        = nh_.createTimer(ros::Rate(_mpc_asynchronous_rate_), &MpcTracker::timerMPC, this, false, false);
+     636          82 :   timer_velocity_tracking_    = nh_.createTimer(ros::Rate(30.0), &MpcTracker::timerVelocityTracking, this, false, false);
+     637          82 :   timer_hover_                = nh_.createTimer(ros::Rate(10.0), &MpcTracker::timerHover, this, false, false);
+     638             : 
+     639             :   // | ----------------------- finish init ---------------------- |
+     640             : 
+     641          82 :   is_initialized_ = true;
+     642             : 
+     643          82 :   ROS_INFO("[MpcTracker]: initialized");
+     644             : 
+     645          82 :   return true;
+     646             : }
+     647             : 
+     648             : //}
+     649             : 
+     650             : /* //{ activate() */
+     651             : 
+     652          75 : std::tuple<bool, std::string> MpcTracker::activate(const std::optional<mrs_msgs::TrackerCommand>& last_tracker_cmd) {
+     653             : 
+     654         150 :   std::stringstream ss;
+     655             : 
+     656          75 :   if (!got_constraints_) {
+     657             : 
+     658           0 :     ss << "can not activate, missing constraints";
+     659           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+     660             : 
+     661           0 :     return std::tuple(false, ss.str());
+     662             :   }
+     663             : 
+     664         150 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     665             : 
+     666             :   double uav_state_heading;
+     667             : 
+     668             :   try {
+     669          75 :     uav_state_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     670             :   }
+     671           0 :   catch (...) {
+     672           0 :     ss << "could not calculate the UAV heading";
+     673           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+     674           0 :     return std::tuple(false, ss.str());
+     675             :   }
+     676             : 
+     677         150 :   MatrixXd mpc_x         = MatrixXd::Zero(MPC_N_STATES, 1);
+     678          75 :   MatrixXd mpc_x_heading = MatrixXd::Zero(MPC_HEADING_N_STATES, 1);
+     679             : 
+     680          75 :   if (last_tracker_cmd) {
+     681             : 
+     682             :     // set the initial condition from the last tracker's cmd
+     683             : 
+     684          72 :     if (last_tracker_cmd->use_position_horizontal) {
+     685          72 :       mpc_x(0, 0) = last_tracker_cmd->position.x;
+     686          72 :       mpc_x(4, 0) = last_tracker_cmd->position.y;
+     687             :     } else {
+     688           0 :       mpc_x(0, 0) = uav_state.pose.position.x;
+     689           0 :       mpc_x(4, 0) = uav_state.pose.position.y;
+     690             :     }
+     691             : 
+     692          72 :     if (last_tracker_cmd->use_position_vertical) {
+     693          72 :       mpc_x(8, 0) = last_tracker_cmd->position.z;
+     694             :     } else {
+     695           0 :       mpc_x(8, 0) = uav_state.pose.position.z;
+     696             :     }
+     697             : 
+     698          72 :     if (last_tracker_cmd->use_velocity_horizontal) {
+     699          72 :       mpc_x(1, 0) = last_tracker_cmd->velocity.x;
+     700          72 :       mpc_x(5, 0) = last_tracker_cmd->velocity.y;
+     701             :     } else {
+     702           0 :       mpc_x(1, 0) = uav_state.velocity.linear.x;
+     703           0 :       mpc_x(5, 0) = uav_state.velocity.linear.y;
+     704             :     }
+     705             : 
+     706          72 :     if (last_tracker_cmd->use_velocity_vertical) {
+     707          72 :       mpc_x(9, 0) = last_tracker_cmd->velocity.z;
+     708             :     } else {
+     709           0 :       mpc_x(9, 0) = uav_state.velocity.linear.z;
+     710             :     }
+     711             : 
+     712          72 :     if (last_tracker_cmd->use_acceleration) {
+     713           0 :       mpc_x(2, 0)  = last_tracker_cmd->acceleration.x;
+     714           0 :       mpc_x(6, 0)  = last_tracker_cmd->acceleration.y;
+     715           0 :       mpc_x(10, 0) = last_tracker_cmd->acceleration.z;
+     716             :     } else {
+     717          72 :       mpc_x(2, 0)  = 0;
+     718          72 :       mpc_x(6, 0)  = 0;
+     719          72 :       mpc_x(10, 0) = 0;
+     720             :     }
+     721             : 
+     722             :     // the jerks
+     723          72 :     mpc_x(3, 0)  = 0;
+     724          72 :     mpc_x(7, 0)  = 0;
+     725          72 :     mpc_x(11, 0) = 0;
+     726             : 
+     727          72 :     if (last_tracker_cmd->use_heading) {
+     728          72 :       mpc_x_heading(0, 0) = last_tracker_cmd->heading;
+     729           0 :     } else if (last_tracker_cmd->use_orientation) {
+     730             :       try {
+     731           0 :         mpc_x_heading(0, 0) = mrs_lib::AttitudeConverter(last_tracker_cmd->orientation).getHeading();
+     732             :       }
+     733           0 :       catch (...) {
+     734           0 :         mpc_x_heading(0, 0) = uav_state_heading;
+     735             :       }
+     736             :     } else {
+     737           0 :       mpc_x_heading(0, 0) = uav_state_heading;
+     738             :     }
+     739             : 
+     740          72 :     if (last_tracker_cmd->use_heading_rate) {
+     741          19 :       mpc_x_heading(1, 0) = last_tracker_cmd->heading_rate;
+     742             :     } else {
+     743          53 :       mpc_x_heading(1, 0) = uav_state.velocity.angular.z;
+     744             :     }
+     745             : 
+     746          72 :     mpc_x_heading(2, 0) = 0;
+     747          72 :     mpc_x_heading(3, 0) = 0;
+     748             : 
+     749          72 :     ROS_INFO("[MpcTracker]: activated with last tracker's command");
+     750             : 
+     751             :   } else {
+     752             : 
+     753             :     // set the initial condition completely from the uav_state
+     754             : 
+     755           3 :     mpc_x(0, 0) = uav_state.pose.position.x;
+     756           3 :     mpc_x(1, 0) = uav_state.velocity.linear.x;
+     757           3 :     mpc_x(2, 0) = 0;
+     758           3 :     mpc_x(3, 0) = 0;
+     759             : 
+     760           3 :     mpc_x(4, 0) = uav_state.pose.position.y;
+     761           3 :     mpc_x(5, 0) = uav_state.velocity.linear.y;
+     762           3 :     mpc_x(6, 0) = 0;
+     763           3 :     mpc_x(7, 0) = 0;
+     764             : 
+     765           3 :     mpc_x(8, 0)  = uav_state.pose.position.z;
+     766           3 :     mpc_x(9, 0)  = uav_state.velocity.linear.z;
+     767           3 :     mpc_x(10, 0) = 0;
+     768           3 :     mpc_x(11, 0) = 0;
+     769             : 
+     770           3 :     mpc_x_heading(0, 0) = uav_state_heading;
+     771           3 :     mpc_x_heading(1, 0) = uav_state.velocity.angular.z;
+     772           3 :     mpc_x_heading(2, 0) = 0;
+     773           3 :     mpc_x_heading(3, 0) = 0;
+     774             : 
+     775           3 :     ROS_INFO("[MpcTracker]: activated with uav state");
+     776             :   }
+     777             : 
+     778             :   {
+     779         150 :     std::scoped_lock lock(mutex_mpc_x_);
+     780             : 
+     781          75 :     mpc_x_         = mpc_x;
+     782          75 :     mpc_x_heading_ = mpc_x_heading;
+     783             :   }
+     784             : 
+     785          75 :   trajectory_tracking_in_progress_ = false;
+     786             : 
+     787          75 :   ss << "activated";
+     788          75 :   ROS_INFO_STREAM("[MpcTracker]: " << ss.str());
+     789             : 
+     790             :   // this is here to initialize the desired_trajectory vector
+     791             :   // if deleted (and I tried) the UAV will briefly fly to the
+     792             :   // origin after activation
+     793          75 :   setRelativeGoal(0, 0, 0, 0, false);  // do not delete
+     794             : 
+     795          75 :   toggleHover(true);
+     796             : 
+     797          75 :   model_first_iteration_ = true;
+     798             : 
+     799          75 :   A_ = _mat_A_;
+     800          75 :   B_ = _mat_B_;
+     801             : 
+     802          75 :   A_heading_ = _mat_A_heading_;
+     803          75 :   B_heading_ = _mat_B_heading_;
+     804             : 
+     805          75 :   is_active_ = true;
+     806             : 
+     807          75 :   if (!mpc_synchronous_) {
+     808          65 :     timer_mpc_iteration_.start();
+     809             :   }
+     810             : 
+     811          75 :   return std::tuple(true, ss.str());
+     812             : }
+     813             : 
+     814             : //}
+     815             : 
+     816             : /* //{ deactivate() */
+     817             : 
+     818          36 : void MpcTracker::deactivate(void) {
+     819             : 
+     820          36 :   toggleHover(false);
+     821             : 
+     822          36 :   is_active_                       = false;
+     823          36 :   trajectory_tracking_in_progress_ = false;
+     824          36 :   model_first_iteration_           = true;
+     825             : 
+     826             :   {
+     827          36 :     std::scoped_lock lock(mutex_trajectory_tracking_states_);
+     828             : 
+     829          36 :     trajectory_current_time_ = 0;
+     830             :   }
+     831             : 
+     832          36 :   ROS_INFO("[MpcTracker]: deactivated");
+     833             : 
+     834          36 :   timer_mpc_iteration_.stop();
+     835             : 
+     836          36 :   publishDiagnostics();
+     837          36 : }
+     838             : 
+     839             : //}
+     840             : 
+     841             : /* //{ resetStatic() */
+     842             : 
+     843           0 : bool MpcTracker::resetStatic(void) {
+     844             : 
+     845           0 :   if (!is_initialized_) {
+     846           0 :     ROS_ERROR("[MpcTracker]: can not reset, not initialized");
+     847           0 :     return false;
+     848             :   }
+     849             : 
+     850           0 :   if (!is_active_) {
+     851           0 :     ROS_ERROR("[MpcTracker]: can not reset, not active");
+     852           0 :     return false;
+     853             :   }
+     854             : 
+     855           0 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     856             : 
+     857             :   double uav_state_heading;
+     858             : 
+     859             :   try {
+     860           0 :     uav_state_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     861             :   }
+     862           0 :   catch (...) {
+     863           0 :     ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: could not calculate the UAV heading");
+     864           0 :     return false;
+     865             :   }
+     866             : 
+     867             :   {
+     868           0 :     std::scoped_lock lock(mutex_mpc_x_);
+     869             : 
+     870             :     // set the initial condition from the odometry
+     871             : 
+     872           0 :     ROS_INFO("[MpcTracker]: reseting with uav state with no dynamics");
+     873             : 
+     874           0 :     mpc_x_(0, 0) = uav_state.pose.position.x;
+     875           0 :     mpc_x_(1, 0) = 0;
+     876           0 :     mpc_x_(2, 0) = 0;
+     877           0 :     mpc_x_(3, 0) = 0;
+     878             : 
+     879           0 :     mpc_x_(4, 0) = uav_state.pose.position.y;
+     880           0 :     mpc_x_(5, 0) = 0;
+     881           0 :     mpc_x_(6, 0) = 0;
+     882           0 :     mpc_x_(7, 0) = 0;
+     883             : 
+     884           0 :     mpc_x_(8, 0)  = uav_state.pose.position.z;
+     885           0 :     mpc_x_(9, 0)  = 0;
+     886           0 :     mpc_x_(10, 0) = 0;
+     887           0 :     mpc_x_(11, 0) = 0;
+     888             : 
+     889           0 :     mpc_x_heading_(0, 0) = uav_state_heading;
+     890           0 :     mpc_x_heading_(1, 0) = 0;
+     891           0 :     mpc_x_heading_(2, 0) = 0;
+     892           0 :     mpc_x_heading_(3, 0) = 0;
+     893             : 
+     894           0 :     trajectory_tracking_in_progress_ = false;
+     895             : 
+     896           0 :     ROS_INFO("[MpcTracker]: reseted");
+     897             :   }
+     898             : 
+     899             :   // this is here to initialize the desired_trajectory vector
+     900             :   // if deleted (and I tried) the UAV will briefly fly to the
+     901             :   // origin after activation
+     902           0 :   setRelativeGoal(0, 0, 0, 0, false);  // do not delete
+     903             : 
+     904           0 :   return true;
+     905             : }
+     906             : 
+     907             : //}
+     908             : 
+     909             : /* //{ update() */
+     910             : 
+     911      111592 : std::optional<mrs_msgs::TrackerCommand> MpcTracker::update(const mrs_msgs::UavState&                                           uav_state,
+     912             :                                                            [[maybe_unused]] const mrs_uav_managers::Controller::ControlOutput& last_control_output) {
+     913             : 
+     914      334776 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("update");
+     915      334776 :   mrs_lib::ScopeTimer timer            = mrs_lib::ScopeTimer("MpcTracker::update", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+     916             : 
+     917      223184 :   auto old_uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+     918             : 
+     919             :   // the time from the last update call
+     920      111592 :   double dt = (uav_state.header.stamp - old_uav_state.header.stamp).toSec();
+     921             : 
+     922             :   // save the uav state
+     923      111592 :   mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_);
+     924             : 
+     925      111592 :   if (dt > 0) {
+     926             : 
+     927      111592 :     double rate = 1.0 / dt;
+     928             : 
+     929      111592 :     update_rate_ = 0.9 * update_rate_ + 0.1 * rate;
+     930             : 
+     931      111592 :     if (mpc_synchronous_ && (update_rate_ > _mpc_synchronous_rate_limit_)) {
+     932           0 :       mpc_synchronous_ = false;
+     933           0 :       ROS_INFO("[MpcTracker]: detecting high update date (%.1f Hz > %.1f Hz), switching to asynchronous mode.", rate, _mpc_synchronous_rate_limit_);
+     934           0 :       if (is_active_) {
+     935           0 :         timer_mpc_iteration_.start();
+     936             :       }
+     937      111592 :     } else if (!mpc_synchronous_ && (update_rate_ <= _mpc_synchronous_rate_limit_)) {
+     938          14 :       mpc_synchronous_ = true;
+     939          14 :       ROS_INFO("[MpcTracker]: detecting low update rate (%.1f Hz < %.1f Hz), switching to synchronous mode.", rate, _mpc_synchronous_rate_limit_);
+     940          14 :       timer_mpc_iteration_.stop();
+     941             :     }
+     942             :   }
+     943             : 
+     944             :   // up to this part the update() method is evaluated even when the tracker is not active
+     945      111592 :   if (!is_active_) {
+     946       51996 :     return {};
+     947             :   }
+     948             : 
+     949      119192 :   mrs_msgs::TrackerCommand tracker_cmd;
+     950             : 
+     951       59596 :   if (!mpc_synchronous_ && (!mpc_computed_ || mpc_result_invalid_)) {
+     952             : 
+     953          57 :     ROS_WARN_THROTTLE(0.1, "[MpcTracker]: MPC not ready, returning current odom as the command");
+     954             : 
+     955             :     // set the header
+     956          57 :     tracker_cmd.header.stamp    = uav_state.header.stamp;
+     957          57 :     tracker_cmd.header.frame_id = uav_state.header.frame_id;
+     958             : 
+     959             :     // set positions from odom
+     960          57 :     tracker_cmd.position.x              = uav_state.pose.position.x;
+     961          57 :     tracker_cmd.position.y              = uav_state.pose.position.y;
+     962          57 :     tracker_cmd.position.z              = uav_state.pose.position.z;
+     963          57 :     tracker_cmd.use_position_vertical   = 1;
+     964          57 :     tracker_cmd.use_position_horizontal = 1;
+     965             : 
+     966             :     // set velocities from odom
+     967          57 :     tracker_cmd.velocity.x              = uav_state.velocity.linear.x;
+     968          57 :     tracker_cmd.velocity.y              = uav_state.velocity.linear.y;
+     969          57 :     tracker_cmd.velocity.z              = uav_state.velocity.linear.z;
+     970          57 :     tracker_cmd.use_velocity_vertical   = 1;
+     971          57 :     tracker_cmd.use_velocity_horizontal = 1;
+     972             : 
+     973             :     // set zero accelerations
+     974          57 :     tracker_cmd.acceleration.x   = 0;
+     975          57 :     tracker_cmd.acceleration.y   = 0;
+     976          57 :     tracker_cmd.acceleration.z   = 0;
+     977          57 :     tracker_cmd.use_acceleration = 1;
+     978             : 
+     979             :     try {
+     980          57 :       tracker_cmd.heading     = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+     981          57 :       tracker_cmd.use_heading = 1;
+     982             :     }
+     983           0 :     catch (...) {
+     984           0 :       tracker_cmd.use_heading = 0;
+     985           0 :       ROS_WARN_THROTTLE(1.0, "[MpcTracker]: could not calculate the current UAV heading");
+     986             :     }
+     987             : 
+     988             :     // set zero jerk
+     989          57 :     tracker_cmd.jerk.x = 0;
+     990          57 :     tracker_cmd.jerk.y = 0;
+     991          57 :     tracker_cmd.jerk.z = 0;
+     992             : 
+     993             :     try {
+     994          57 :       tracker_cmd.heading_rate     = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeadingRate(uav_state.velocity.angular);
+     995          57 :       tracker_cmd.use_heading_rate = 1;
+     996             :     }
+     997           0 :     catch (...) {
+     998           0 :       tracker_cmd.use_heading_rate = 0;
+     999           0 :       ROS_WARN_THROTTLE(1.0, "[MpcTracker]: could not calculate the current UAV heading rate");
+    1000             :     }
+    1001             : 
+    1002          57 :     return {tracker_cmd};
+    1003             :   }
+    1004             : 
+    1005       59539 :   if (mpc_synchronous_) {
+    1006             : 
+    1007        3628 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: running in SYNCHRONOUS mode");
+    1008             : 
+    1009        3628 :     if (synchronous_timer_event_.last_real.toSec() > 0) {
+    1010        3614 :       synchronous_timer_event_.last_real = synchronous_timer_event_.current_real;
+    1011             :     } else {
+    1012          14 :       synchronous_timer_event_.last_real = ros::Time::now();
+    1013             :     }
+    1014             : 
+    1015        3628 :     synchronous_timer_event_.current_real = ros::Time::now();
+    1016             : 
+    1017        3628 :     timerMPC(synchronous_timer_event_);
+    1018             : 
+    1019             :   } else {
+    1020       55911 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: running in ASYNCHRONOUS mode");
+    1021             :   }
+    1022             : 
+    1023       59539 :   if (dt > 0) {
+    1024       59539 :     iterateModel(dt);
+    1025             :   } else {
+    1026           0 :     ROS_WARN_THROTTLE(1.0, "[MpcTracker]: dt !> 0, not iterating the model");
+    1027             :   }
+    1028             : 
+    1029      119078 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1030      119078 :   auto prediction_full_state  = mrs_lib::get_mutexed(mutex_prediction_full_state_, prediction_full_state_);
+    1031             : 
+    1032             :   // check whether all outputs are finite
+    1033       59539 :   bool arefinite = true;
+    1034      774007 :   for (int i = 0; i < 12; i++) {
+    1035      714468 :     if (!std::isfinite(mpc_x(i, 0))) {
+    1036           0 :       arefinite = false;
+    1037             :     }
+    1038             :   }
+    1039             : 
+    1040       59539 :   if (arefinite) {
+    1041             : 
+    1042             :     // set the desired states base on the result of the mpc
+    1043       59539 :     tracker_cmd.position.x     = mpc_x(0, 0);
+    1044       59539 :     tracker_cmd.velocity.x     = mpc_x(1, 0);
+    1045       59539 :     tracker_cmd.acceleration.x = mpc_x(2, 0);
+    1046       59539 :     tracker_cmd.jerk.x         = mpc_x(3, 0);
+    1047             : 
+    1048       59539 :     tracker_cmd.position.y     = mpc_x(4, 0);
+    1049       59539 :     tracker_cmd.velocity.y     = mpc_x(5, 0);
+    1050       59539 :     tracker_cmd.acceleration.y = mpc_x(6, 0);
+    1051       59539 :     tracker_cmd.jerk.y         = mpc_x(7, 0);
+    1052             : 
+    1053       59539 :     tracker_cmd.position.z     = mpc_x(8, 0);
+    1054       59539 :     tracker_cmd.velocity.z     = mpc_x(9, 0);
+    1055       59539 :     tracker_cmd.acceleration.z = mpc_x(10, 0);
+    1056       59539 :     tracker_cmd.jerk.z         = mpc_x(11, 0);
+    1057             : 
+    1058       59539 :     tracker_cmd.full_state_prediction = prediction_full_state;
+    1059             : 
+    1060       59539 :     tracker_cmd.use_position_vertical     = 1;
+    1061       59539 :     tracker_cmd.use_position_horizontal   = 1;
+    1062       59539 :     tracker_cmd.use_velocity_vertical     = 1;
+    1063       59539 :     tracker_cmd.use_velocity_horizontal   = 1;
+    1064       59539 :     tracker_cmd.use_acceleration          = 1;
+    1065       59539 :     tracker_cmd.use_jerk                  = 1;
+    1066       59539 :     tracker_cmd.use_full_state_prediction = 1;
+    1067             : 
+    1068             :   } else {
+    1069             : 
+    1070           0 :     ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: MPC translation outputs are not finite!");
+    1071             : 
+    1072           0 :     return {};
+    1073             :   }
+    1074             : 
+    1075       59539 :   bool heading_finite = true;
+    1076      297695 :   for (int i = 0; i < MPC_HEADING_N_STATES; i++) {
+    1077      238156 :     if (!std::isfinite(mpc_x_heading(i, 0))) {
+    1078           0 :       heading_finite = false;
+    1079             :     }
+    1080             :   }
+    1081             : 
+    1082       59539 :   if (heading_finite) {
+    1083             : 
+    1084       59539 :     tracker_cmd.heading              = mpc_x_heading(0, 0);
+    1085       59539 :     tracker_cmd.heading_rate         = mpc_x_heading(1, 0);
+    1086       59539 :     tracker_cmd.heading_acceleration = mpc_x_heading(2, 0);
+    1087       59539 :     tracker_cmd.heading_jerk         = mpc_x_heading(3, 0);
+    1088             : 
+    1089       59539 :     tracker_cmd.use_heading              = 1;
+    1090       59539 :     tracker_cmd.use_heading_rate         = 1;
+    1091       59539 :     tracker_cmd.use_heading_acceleration = 1;
+    1092       59539 :     tracker_cmd.use_heading_jerk         = 1;
+    1093             : 
+    1094             :   } else {
+    1095             : 
+    1096           0 :     ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: MPC heading output is not finite!");
+    1097             : 
+    1098           0 :     return {};
+    1099             :   }
+    1100             : 
+    1101             :   // set the header
+    1102       59539 :   tracker_cmd.header.stamp    = uav_state.header.stamp;
+    1103       59539 :   tracker_cmd.header.frame_id = uav_state.header.frame_id;
+    1104             : 
+    1105             :   // u have to return a position command
+    1106             :   // can set the jerk to 0
+    1107       59539 :   return {tracker_cmd};
+    1108             : }  // namespace mpc_tracker
+    1109             : 
+    1110             : //}
+    1111             : 
+    1112             : /* //{ getStatus() */
+    1113             : 
+    1114        6957 : const mrs_msgs::TrackerStatus MpcTracker::getStatus() {
+    1115             : 
+    1116       13914 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1117        6957 :   auto trajectory_size        = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_size_);
+    1118             : 
+    1119             :   double des_x, des_y, des_z, des_heading;
+    1120             :   {
+    1121        6957 :     std::scoped_lock lock(mutex_des_trajectory_);
+    1122             : 
+    1123        6957 :     des_x       = des_x_trajectory_(0);
+    1124        6957 :     des_y       = des_y_trajectory_(0);
+    1125        6957 :     des_z       = des_z_trajectory_(0);
+    1126        6957 :     des_heading = des_heading_trajectory_(0);
+    1127             :   }
+    1128             : 
+    1129        6957 :   mrs_msgs::TrackerStatus tracker_status;
+    1130             : 
+    1131        6957 :   tracker_status.active            = is_active_;
+    1132        6957 :   tracker_status.callbacks_enabled = is_active_ && callbacks_enabled_ && !hovering_in_progress_;
+    1133             : 
+    1134        6957 :   tracker_status.tracking_trajectory = trajectory_tracking_in_progress_;
+    1135             : 
+    1136        6957 :   bool have_position_error   = sqrt(pow(mpc_x(0, 0) - des_x, 2) + pow(mpc_x(4, 0) - des_y, 2) + pow(mpc_x(8, 0) - des_z, 2)) > _diag_pos_tracking_thr_;
+    1137        6957 :   bool have_heading_error    = fabs(radians::diff(mpc_x_heading(0), des_heading)) > _diag_heading_tracking_thr_;
+    1138        6957 :   bool have_nonzero_velocity = fabs(mpc_x(1, 0)) > 0.1 || fabs(mpc_x(5, 0)) > 0.1 || fabs(mpc_x(9, 0)) > 0.1 || fabs(mpc_x_heading(1, 0)) > 0.1;
+    1139             : 
+    1140        6957 :   tracker_status.have_goal = trajectory_tracking_in_progress_ || hovering_in_progress_ || have_position_error || have_heading_error || have_nonzero_velocity;
+    1141             : 
+    1142        6957 :   int trajectory_tracking_idx = getCurrentTrajectoryIdx();
+    1143             : 
+    1144        6957 :   tracker_status.trajectory_length = trajectory_size;
+    1145        6957 :   tracker_status.trajectory_idx    = trajectory_tracking_idx;
+    1146             : 
+    1147        6957 :   if (trajectory_tracking_in_progress_) {
+    1148             : 
+    1149        2658 :     auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1150             : 
+    1151        2658 :     std::scoped_lock lock(mutex_des_whole_trajectory_);
+    1152             : 
+    1153        1329 :     tracker_status.trajectory_reference.header.stamp    = ros::Time::now();
+    1154        1329 :     tracker_status.trajectory_reference.header.frame_id = uav_state.header.frame_id;
+    1155             : 
+    1156        1329 :     tracker_status.trajectory_reference.reference.position.x = (*des_x_whole_trajectory_)(trajectory_tracking_idx);
+    1157        1329 :     tracker_status.trajectory_reference.reference.position.y = (*des_y_whole_trajectory_)(trajectory_tracking_idx);
+    1158        1329 :     tracker_status.trajectory_reference.reference.position.z = (*des_z_whole_trajectory_)(trajectory_tracking_idx);
+    1159        1329 :     tracker_status.trajectory_reference.reference.heading    = (*des_heading_whole_trajectory_)(trajectory_tracking_idx);
+    1160             : 
+    1161             :     // | ---------- publish the current trajectory point ---------- |
+    1162             : 
+    1163        2658 :     geometry_msgs::PoseStamped debug_trajectory_point;
+    1164        1329 :     debug_trajectory_point.header.stamp    = ros::Time::now();
+    1165        1329 :     debug_trajectory_point.header.frame_id = uav_state_.header.frame_id;
+    1166             : 
+    1167        1329 :     debug_trajectory_point.pose.position.x = (*des_x_whole_trajectory_)(trajectory_tracking_idx);
+    1168        1329 :     debug_trajectory_point.pose.position.y = (*des_y_whole_trajectory_)(trajectory_tracking_idx);
+    1169        1329 :     debug_trajectory_point.pose.position.z = (*des_z_whole_trajectory_)(trajectory_tracking_idx);
+    1170             : 
+    1171        1329 :     debug_trajectory_point.pose.orientation = mrs_lib::AttitudeConverter(0, 0, (*des_heading_whole_trajectory_)(trajectory_tracking_idx));
+    1172             : 
+    1173        1329 :     ph_current_trajectory_point_.publish(debug_trajectory_point);
+    1174             :   }
+    1175             : 
+    1176       13914 :   return tracker_status;
+    1177             : }
+    1178             : 
+    1179             : //}
+    1180             : 
+    1181             : /* //{ enableCallbacks() */
+    1182             : 
+    1183        1619 : const std_srvs::SetBoolResponse::ConstPtr MpcTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr& cmd) {
+    1184             : 
+    1185        3238 :   std::stringstream ss;
+    1186             : 
+    1187        1619 :   if (cmd->data != callbacks_enabled_) {
+    1188             : 
+    1189        1353 :     callbacks_enabled_ = cmd->data;
+    1190        1353 :     ss << "callbacks %s" << (callbacks_enabled_ ? "enabled" : "disabled");
+    1191             : 
+    1192             :   } else {
+    1193             : 
+    1194         266 :     ss << "callbacks were already %s" << (callbacks_enabled_ ? "enabled" : "disabled");
+    1195             :   }
+    1196             : 
+    1197        3238 :   std_srvs::SetBoolResponse res;
+    1198        1619 :   res.message = ss.str();
+    1199        1619 :   res.success = true;
+    1200             : 
+    1201        3238 :   return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res));
+    1202             : }
+    1203             : 
+    1204             : //}
+    1205             : 
+    1206             : /* switchOdometrySource() //{ */
+    1207             : 
+    1208           4 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::switchOdometrySource(const mrs_msgs::UavState& new_uav_state) {
+    1209             : 
+    1210           4 :   odometry_reset_in_progress_ = true;
+    1211           4 :   mpc_result_invalid_         = true;
+    1212             : 
+    1213           8 :   auto x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1214           8 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1215             : 
+    1216           4 :   ROS_INFO(
+    1217             :       "[MpcTracker]: start of odmetry reset, curent state [x: %.2f, y: %.2f, z: %.2f] [x_d: %.2f, y_d: %.2f, z_d: %.2f] [x_dd: %.2f, y_dd: %.2f, z_dd: "
+    1218             :       "%.2f], "
+    1219             :       "new odom [x: %.2f, y: %.2f, z: %.2f] [x_d: %.2f, y_d: %.2f, z_d: %.2f] [x_dd: %.2f, y_dd: %.2f, z_dd: %.2f]",
+    1220             :       x(0, 0), x(4, 0), x(8, 0), x(1, 0), x(5, 0), x(9, 0), x(2, 0), x(6, 0), x(10, 0), new_uav_state.pose.position.x, new_uav_state.pose.position.y,
+    1221             :       new_uav_state.pose.position.z, new_uav_state.velocity.linear.x, new_uav_state.velocity.linear.y, new_uav_state.velocity.linear.z,
+    1222             :       new_uav_state.acceleration.linear.x, new_uav_state.acceleration.linear.y, new_uav_state.acceleration.linear.z);
+    1223             : 
+    1224           4 :   timer_mpc_iteration_.stop();
+    1225           4 :   ROS_INFO("[MpcTracker]: mpc timer stopped");
+    1226             : 
+    1227           4 :   while (mpc_timer_running_) {
+    1228             : 
+    1229           3 :     ROS_DEBUG("[MpcTracker]: the mpc is in the middle of an iteration, waiting for it to finish");
+    1230           3 :     ros::Duration wait(0.001);
+    1231           3 :     wait.sleep();
+    1232             : 
+    1233           3 :     if (!mpc_timer_running_) {
+    1234           3 :       ROS_DEBUG("[MpcTracker]: mpc timer finished");
+    1235           3 :       break;
+    1236             :     }
+    1237             :   }
+    1238             : 
+    1239             :   // | --------- recalculate the goal to new coordinates -------- |
+    1240             : 
+    1241           4 :   double old_heading  = 0;
+    1242           4 :   double new_heading  = 0;
+    1243           4 :   bool   got_headings = true;
+    1244             : 
+    1245             :   try {
+    1246           4 :     old_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+    1247             :   }
+    1248           0 :   catch (...) {
+    1249           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the old UAV heading");
+    1250           0 :     got_headings = false;
+    1251             :   }
+    1252             : 
+    1253             :   try {
+    1254           4 :     new_heading = mrs_lib::AttitudeConverter(new_uav_state.pose.orientation).getHeading();
+    1255             :   }
+    1256           0 :   catch (...) {
+    1257           0 :     ROS_ERROR_THROTTLE(1.0, "[LineTracker]: could not calculate the new UAV heading");
+    1258           0 :     got_headings = false;
+    1259             :   }
+    1260             : 
+    1261           8 :   std_srvs::TriggerResponse res;
+    1262             : 
+    1263           4 :   if (!got_headings) {
+    1264           0 :     res.message = "could not calculate the heading difference";
+    1265           0 :     res.success = false;
+    1266             : 
+    1267           0 :     return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1268             :   }
+    1269             : 
+    1270             :   // calculate the difference of position
+    1271           4 :   double dx       = new_uav_state.pose.position.x - uav_state_.pose.position.x;
+    1272           4 :   double dy       = new_uav_state.pose.position.y - uav_state_.pose.position.y;
+    1273           4 :   double dz       = new_uav_state.pose.position.z - uav_state_.pose.position.z;
+    1274           4 :   double dheading = new_heading - old_heading;
+    1275             : 
+    1276           4 :   ROS_INFO("[MpcTracker]: dx %f dy %f dz %f dheading %f", dx, dy, dz, dheading);
+    1277             : 
+    1278             :   {
+    1279           4 :     std::scoped_lock lock(mutex_mpc_x_, mutex_des_trajectory_, mutex_des_whole_trajectory_, mutex_uav_state_);
+    1280             : 
+    1281           4 :     if (trajectory_set_) {
+    1282             : 
+    1283           0 :       for (int i = 0; i < trajectory_size_ + MPC_HORIZON_LENGTH; i++) {
+    1284             : 
+    1285           0 :         Eigen::Vector2d temp_vec((*des_x_whole_trajectory_)(i)-uav_state_.pose.position.x, (*des_y_whole_trajectory_)(i)-uav_state_.pose.position.y);
+    1286           0 :         temp_vec = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1287             : 
+    1288           0 :         (*des_x_whole_trajectory_)(i) = new_uav_state.pose.position.x + temp_vec[0];
+    1289           0 :         (*des_y_whole_trajectory_)(i) = new_uav_state.pose.position.y + temp_vec[1];
+    1290           0 :         (*des_z_whole_trajectory_)(i) += dz;
+    1291           0 :         (*des_heading_whole_trajectory_)(i) += dheading;
+    1292             :       }
+    1293             :     }
+    1294             : 
+    1295         164 :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    1296             : 
+    1297         160 :       Eigen::Vector2d temp_vec(des_x_trajectory_(i) - uav_state_.pose.position.x, des_y_trajectory_(i) - uav_state_.pose.position.y);
+    1298         160 :       temp_vec = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1299             : 
+    1300         160 :       des_x_trajectory_(i, 0) = new_uav_state.pose.position.x + temp_vec[0];
+    1301         160 :       des_y_trajectory_(i, 0) = new_uav_state.pose.position.y + temp_vec[1];
+    1302         160 :       des_z_trajectory_(i, 0) += dz;
+    1303         160 :       des_heading_trajectory_(i, 0) += dheading;
+    1304             :     }
+    1305             : 
+    1306             :     // update the position
+    1307             :     {
+    1308           4 :       Eigen::Vector2d temp_vec(mpc_x_(0, 0) - uav_state_.pose.position.x, mpc_x_(4, 0) - uav_state_.pose.position.y);
+    1309           4 :       temp_vec     = Eigen::Rotation2D<double>(dheading).toRotationMatrix() * temp_vec;
+    1310           4 :       mpc_x_(0, 0) = new_uav_state.pose.position.x + temp_vec[0];
+    1311           4 :       mpc_x_(4, 0) = new_uav_state.pose.position.y + temp_vec[1];
+    1312           4 :       mpc_x_(8, 0) += dz;
+    1313             :     }
+    1314             : 
+    1315             :     // update the velocity
+    1316             :     {
+    1317           4 :       mpc_x_(1, 0) = new_uav_state.velocity.linear.x;
+    1318           4 :       mpc_x_(5, 0) = new_uav_state.velocity.linear.y;
+    1319             :       // we leave the z velocity as it was in the original frame
+    1320             :     }
+    1321             : 
+    1322             :     // update the acceleration
+    1323             :     {
+    1324           4 :       mpc_x_(2, 0)  = 0;
+    1325           4 :       mpc_x_(6, 0)  = 0;
+    1326           4 :       mpc_x_(10, 0) = 0;
+    1327             :     }
+    1328             : 
+    1329             :     // update the heading and its derivative
+    1330           4 :     mpc_x_heading_(0, 0) += dheading;
+    1331           4 :     mpc_x_heading_(1, 0) = new_uav_state.velocity.angular.x;
+    1332             :   }
+    1333             : 
+    1334           4 :   ROS_INFO(
+    1335             :       "[MpcTracker]: start of odmetry reset, curent state [x: %.2f, y: %.2f, z: %.2f] [x_d: %.2f, y_d: %.2f, z_d: %.2f] [x_dd: %.2f, y_dd: %.2f, z_dd: "
+    1336             :       "%.2f]",
+    1337             :       x(0, 0), x(4, 0), x(8, 0), x(1, 0), x(5, 0), x(9, 0), x(2, 0), x(6, 0), x(10, 0));
+    1338             : 
+    1339           4 :   ROS_INFO("[MpcTracker]: starting the MPC timer");
+    1340             : 
+    1341           4 :   if (!mpc_synchronous_) {
+    1342           4 :     timer_mpc_iteration_.start();
+    1343             :   }
+    1344             : 
+    1345           4 :   odometry_reset_in_progress_ = false;
+    1346             : 
+    1347           4 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1348             : }
+    1349             : 
+    1350             : //}
+    1351             : 
+    1352             : /* //{ hover() */
+    1353             : 
+    1354           0 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1355             : 
+    1356           0 :   toggleHover(true);
+    1357             : 
+    1358           0 :   std::stringstream ss;
+    1359           0 :   ss << "initiating hover";
+    1360             : 
+    1361           0 :   std_srvs::TriggerResponse res;
+    1362           0 :   res.success = true;
+    1363           0 :   res.message = ss.str();
+    1364             : 
+    1365           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1366             : }
+    1367             : 
+    1368             : //}
+    1369             : 
+    1370             : /* //{ startTrajectoryTracking() */
+    1371             : 
+    1372           1 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1373           2 :   std::stringstream ss;
+    1374             : 
+    1375           2 :   auto [success, message] = startTrajectoryTrackingImpl();
+    1376             : 
+    1377           2 :   std_srvs::TriggerResponse res;
+    1378           1 :   res.success = success;
+    1379           1 :   res.message = message;
+    1380             : 
+    1381           2 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1382             : }
+    1383             : 
+    1384             : //}
+    1385             : 
+    1386             : /* //{ stopTrajectoryTracking() */
+    1387             : 
+    1388           0 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1389             : 
+    1390           0 :   auto [success, message] = stopTrajectoryTrackingImpl();
+    1391             : 
+    1392           0 :   std_srvs::TriggerResponse res;
+    1393           0 :   res.success = success;
+    1394           0 :   res.message = message;
+    1395             : 
+    1396           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1397             : }
+    1398             : 
+    1399             : //}
+    1400             : 
+    1401             : /* //{ resumeTrajectoryTracking() */
+    1402             : 
+    1403           0 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1404             : 
+    1405           0 :   auto [success, message] = resumeTrajectoryTrackingImpl();
+    1406             : 
+    1407           0 :   std_srvs::TriggerResponse res;
+    1408           0 :   res.success = success;
+    1409           0 :   res.message = message;
+    1410             : 
+    1411           0 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1412             : }
+    1413             : 
+    1414             : //}
+    1415             : 
+    1416             : /* //{ gotoTrajectoryStart() */
+    1417             : 
+    1418           1 : const std_srvs::TriggerResponse::ConstPtr MpcTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr& cmd) {
+    1419             : 
+    1420           2 :   auto [success, message] = gotoTrajectoryStartImpl();
+    1421             : 
+    1422           2 :   std_srvs::TriggerResponse res;
+    1423           1 :   res.success = success;
+    1424           1 :   res.message = message;
+    1425             : 
+    1426           2 :   return std_srvs::TriggerResponse::ConstPtr(new std_srvs::TriggerResponse(res));
+    1427             : }
+    1428             : 
+    1429             : //}
+    1430             : 
+    1431             : /* //{ setConstraints() */
+    1432             : 
+    1433         255 : const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr MpcTracker::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& constraints) {
+    1434             : 
+    1435         255 :   if (!is_initialized_) {
+    1436           0 :     return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse());
+    1437             :   }
+    1438             : 
+    1439         255 :   mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_);
+    1440             : 
+    1441             :   // directly updated the speeds in the constraints
+    1442             :   // the reset needs to wait for manageConstraints()
+    1443             :   {
+    1444         510 :     std::scoped_lock lock(mutex_constraints_filtered_);
+    1445             : 
+    1446             :     // important! this needs to be done to initialize the full struct
+    1447         255 :     if (!got_constraints_) {
+    1448             : 
+    1449          82 :       constraints_filtered_ = constraints->constraints;
+    1450             : 
+    1451             :     } else {
+    1452             : 
+    1453         173 :       constraints_filtered_.horizontal_speed          = constraints->constraints.horizontal_speed;
+    1454         173 :       constraints_filtered_.vertical_ascending_speed  = constraints->constraints.vertical_ascending_speed;
+    1455         173 :       constraints_filtered_.vertical_descending_speed = constraints->constraints.vertical_descending_speed;
+    1456         173 :       constraints_filtered_.heading_speed             = constraints->constraints.heading_speed;
+    1457             :     }
+    1458             :   }
+    1459             : 
+    1460         255 :   got_constraints_ = true;
+    1461             : 
+    1462         255 :   all_constraints_set_ = false;
+    1463             : 
+    1464         255 :   ROS_INFO("[MpcTracker]: updating constraints");
+    1465             : 
+    1466         510 :   mrs_msgs::DynamicsConstraintsSrvResponse res;
+    1467         255 :   res.success = true;
+    1468         255 :   res.message = "constraints updated";
+    1469             : 
+    1470         255 :   return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res));
+    1471             : }
+    1472             : 
+    1473             : //}
+    1474             : 
+    1475             : /* //{ setReference() */
+    1476             : 
+    1477         244 : const mrs_msgs::ReferenceSrvResponse::ConstPtr MpcTracker::setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr& cmd) {
+    1478             : 
+    1479         244 :   toggleHover(false);
+    1480             : 
+    1481         244 :   setGoal(cmd->reference.position.x, cmd->reference.position.y, cmd->reference.position.z, cmd->reference.heading, true);
+    1482             : 
+    1483         488 :   mrs_msgs::ReferenceSrvResponse res;
+    1484         244 :   res.success = true;
+    1485         244 :   res.message = "reference set";
+    1486             : 
+    1487         488 :   return mrs_msgs::ReferenceSrvResponse::ConstPtr(new mrs_msgs::ReferenceSrvResponse(res));
+    1488             : }
+    1489             : 
+    1490             : //}
+    1491             : 
+    1492             : /* //{ setVelocityReference() */
+    1493             : 
+    1494         471 : const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr MpcTracker::setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr& cmd) {
+    1495             : 
+    1496         471 :   if (!is_initialized_) {
+    1497           0 :     return mrs_msgs::VelocityReferenceSrvResponse::Ptr();
+    1498             :   }
+    1499             : 
+    1500             :   {
+    1501         471 :     std::scoped_lock lock(mutex_velocity_reference_);
+    1502             : 
+    1503         471 :     velocity_reference_time_ = ros::Time::now();
+    1504             : 
+    1505         471 :     velocity_reference_ = cmd->reference;
+    1506             :   }
+    1507             : 
+    1508         471 :   if (!velocity_tracking_active_) {
+    1509             : 
+    1510           2 :     ROS_INFO("[MpcTracker]: starting velocity tracking timer");
+    1511             : 
+    1512           2 :     timer_velocity_tracking_.stop();
+    1513           2 :     timer_velocity_tracking_.start();
+    1514             : 
+    1515           2 :     velocity_tracking_active_ = true;
+    1516             :   }
+    1517             : 
+    1518         942 :   mrs_msgs::VelocityReferenceSrvResponse response;
+    1519         471 :   response.success = true;
+    1520         471 :   response.message = "reference set";
+    1521             : 
+    1522         471 :   return mrs_msgs::VelocityReferenceSrvResponse::ConstPtr(new mrs_msgs::VelocityReferenceSrvResponse(response));
+    1523             : }
+    1524             : 
+    1525             : //}
+    1526             : 
+    1527             : /* //{ setTrajectoryReference() */
+    1528             : 
+    1529           4 : const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr MpcTracker::setTrajectoryReference([
+    1530             :     [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr& cmd) {
+    1531             : 
+    1532           8 :   std::stringstream ss;
+    1533             : 
+    1534          12 :   auto [success, message, modified] = loadTrajectory(cmd->trajectory);
+    1535             : 
+    1536           8 :   mrs_msgs::TrajectoryReferenceSrvResponse response;
+    1537           4 :   response.success  = success;
+    1538           4 :   response.message  = message;
+    1539           4 :   response.modified = modified;
+    1540             : 
+    1541           8 :   return mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr(new mrs_msgs::TrajectoryReferenceSrvResponse(response));
+    1542             : }
+    1543             : 
+    1544             : //}
+    1545             : 
+    1546             : // | ------------------------ callbacks ----------------------- |
+    1547             : 
+    1548             : /* //{ callbackOtherMavTrajectory() */
+    1549             : 
+    1550         294 : void MpcTracker::callbackOtherMavTrajectory(const mrs_msgs::FutureTrajectory::ConstPtr msg) {
+    1551             : 
+    1552         294 :   if (!is_initialized_) {
+    1553           0 :     return;
+    1554             :   }
+    1555             : 
+    1556         588 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("callbackOtherMavTrajectory");
+    1557             :   mrs_lib::ScopeTimer timer =
+    1558         588 :       mrs_lib::ScopeTimer("MpcTracker::callbackOtherMavTrajectory", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1559             : 
+    1560         294 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    1561             : 
+    1562         294 :   mrs_msgs::FutureTrajectory trajectory = *msg;
+    1563             : 
+    1564             :   // the times might not be synchronized, so just remember the time of receiving it
+    1565         294 :   trajectory.stamp = ros::Time::now();
+    1566             : 
+    1567             :   // transform it from the utm origin to the currently used frame
+    1568         588 :   auto res = common_handlers_->transformer->getTransform("utm_origin", uav_state.header.frame_id, ros::Time::now());
+    1569             : 
+    1570         294 :   if (!res) {
+    1571             : 
+    1572           0 :     std::string message = "[MpcTracker]: can not transform other drone trajectory to the current frame";
+    1573           0 :     ROS_WARN_STREAM_ONCE(message);
+    1574           0 :     ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    1575             : 
+    1576           0 :     return;
+    1577             :   }
+    1578             : 
+    1579         294 :   geometry_msgs::TransformStamped tf = res.value();
+    1580             : 
+    1581       12054 :   for (int i = 0; i < int(trajectory.points.size()); i++) {
+    1582             : 
+    1583       11760 :     geometry_msgs::PoseStamped original_pose;
+    1584             : 
+    1585       11760 :     original_pose.pose.position.x = trajectory.points[i].x;
+    1586       11760 :     original_pose.pose.position.y = trajectory.points[i].y;
+    1587       11760 :     original_pose.pose.position.z = trajectory.points[i].z;
+    1588             : 
+    1589       11760 :     original_pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    1590             : 
+    1591       11760 :     auto res = common_handlers_->transformer->transform(original_pose, tf);
+    1592             : 
+    1593       11760 :     if (res) {
+    1594       11760 :       trajectory.points[i].x = res.value().pose.position.x;
+    1595       11760 :       trajectory.points[i].y = res.value().pose.position.y;
+    1596       11760 :       trajectory.points[i].z = res.value().pose.position.z;
+    1597             :     } else {
+    1598             : 
+    1599           0 :       std::string message = "[MpcTracker]: could not transform point of other uav future trajectory!";
+    1600           0 :       ROS_WARN_STREAM_ONCE(message);
+    1601           0 :       ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    1602             : 
+    1603           0 :       return;
+    1604             :     }
+    1605             :   }
+    1606             : 
+    1607             :   {
+    1608         588 :     std::scoped_lock lock(mutex_other_uav_avoidance_trajectories_);
+    1609             : 
+    1610             :     // update the diagnostics
+    1611         294 :     other_uav_avoidance_trajectories_[trajectory.uav_name] = trajectory;
+    1612             :   }
+    1613             : }
+    1614             : 
+    1615             : //}
+    1616             : 
+    1617             : /* //{ callbackOtherMavDiagnostics() */
+    1618             : 
+    1619        1951 : void MpcTracker::callbackOtherMavDiagnostics(const mrs_msgs::MpcTrackerDiagnostics::ConstPtr msg) {
+    1620             : 
+    1621        5853 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("callbackOtherMavDiagnostics");
+    1622             :   mrs_lib::ScopeTimer timer =
+    1623        5853 :       mrs_lib::ScopeTimer("MpcTracker::callbackOtherMavDiagnostics", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    1624             : 
+    1625        3902 :   std::scoped_lock lock(mutex_other_uav_diagnostics_);
+    1626             : 
+    1627        3902 :   mrs_msgs::MpcTrackerDiagnostics diagnostics = *msg;
+    1628             : 
+    1629             :   // fill in the current time
+    1630             :   // the other uav's time might not be synchronized with ours
+    1631        1951 :   diagnostics.header.stamp = ros::Time::now();
+    1632             : 
+    1633             :   // update the diagnostics
+    1634        1951 :   other_uav_diagnostics_[diagnostics.uav_name] = diagnostics;
+    1635        1951 : }
+    1636             : 
+    1637             : //}
+    1638             : 
+    1639             : /* //{ callbackToggleCollisionAvoidance() */
+    1640             : 
+    1641           0 : bool MpcTracker::callbackToggleCollisionAvoidance(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    1642             : 
+    1643           0 :   collision_avoidance_enabled_ = req.data;
+    1644             : 
+    1645           0 :   ROS_INFO("[MpcTracker]: Collision avoidance was switched %s", collision_avoidance_enabled_ ? "TRUE" : "FALSE");
+    1646             : 
+    1647           0 :   res.message = "Collision avoidance set.";
+    1648           0 :   res.success = true;
+    1649             : 
+    1650           0 :   return true;
+    1651             : }
+    1652             : 
+    1653             : //}
+    1654             : 
+    1655             : /* callbackWiggle() //{ */
+    1656             : 
+    1657           0 : bool MpcTracker::callbackWiggle(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+    1658             : 
+    1659           0 :   if (!is_initialized_) {
+    1660             : 
+    1661           0 :     res.success = false;
+    1662           0 :     res.message = "tracker not active";
+    1663           0 :     return true;
+    1664             :   }
+    1665             : 
+    1666             :   {
+    1667           0 :     std::scoped_lock lock(mutex_drs_params_);
+    1668             : 
+    1669           0 :     drs_params_.wiggle_enabled = req.data;
+    1670             : 
+    1671           0 :     reconfigure_server_->updateConfig(drs_params_);
+    1672             :   }
+    1673             : 
+    1674           0 :   res.success = true;
+    1675           0 :   res.message = "wiggle updated";
+    1676             : 
+    1677           0 :   return true;
+    1678             : }
+    1679             : 
+    1680             : //}
+    1681             : 
+    1682             : /* //{ dynamicReconfigureCallback() */
+    1683             : 
+    1684          82 : void MpcTracker::dynamicReconfigureCallback(mrs_uav_trackers::mpc_trackerConfig& config, [[maybe_unused]] uint32_t level) {
+    1685             : 
+    1686         164 :   std::scoped_lock lock(mutex_drs_params_);
+    1687             : 
+    1688          82 :   drs_params_ = config;
+    1689             : 
+    1690          82 :   ROS_INFO("[MpcTracker]: DRS updated");
+    1691          82 : }
+    1692             : 
+    1693             : //}
+    1694             : 
+    1695             : // --------------------------------------------------------------
+    1696             : // |                          routines                          |
+    1697             : // --------------------------------------------------------------
+    1698             : 
+    1699             : // | --------------- mutual collision avoidance --------------- |
+    1700             : 
+    1701             : /* //{ checkCollision() */
+    1702             : 
+    1703      148640 : bool MpcTracker::checkCollision(const double ax, const double ay, const double az, const double bx, const double by, const double bz) {
+    1704             : 
+    1705      148640 :   if (mrs_lib::geometry::dist(vec2_t(ax, ay), vec2_t(bx, by)) < _avoidance_radius_threshold_ && fabs(az - bz) < _avoidance_z_threshold_) {
+    1706             : 
+    1707       22350 :     return true;
+    1708             : 
+    1709             :   } else {
+    1710             : 
+    1711      126290 :     return false;
+    1712             :   }
+    1713             : }
+    1714             : 
+    1715             : //}
+    1716             : 
+    1717             : /* //{ checkCollisionInflated() */
+    1718             : 
+    1719      148640 : bool MpcTracker::checkCollisionInflated(const double ax, const double ay, const double az, const double bx, const double by, const double bz) {
+    1720             : 
+    1721      148640 :   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) {
+    1722             : 
+    1723      148640 :     return true;
+    1724             : 
+    1725             :   } else {
+    1726             : 
+    1727           0 :     return false;
+    1728             :   }
+    1729             : }
+    1730             : 
+    1731             : //}
+    1732             : 
+    1733             : /* //{ checkTrajectoryForCollisions() */
+    1734             : 
+    1735             : // Check for potential collisions and return the needed altitude offset to avoid other drones
+    1736       64312 : double MpcTracker::checkTrajectoryForCollisions(int& first_collision_index) {
+    1737             : 
+    1738       64312 :   std::scoped_lock lock(mutex_predicted_trajectory_, mutex_des_trajectory_, mutex_other_uav_avoidance_trajectories_);
+    1739             : 
+    1740       64312 :   first_collision_index = INT_MAX;
+    1741       64312 :   avoiding_collision_   = false;
+    1742             : 
+    1743       64312 :   std::map<std::string, mrs_msgs::FutureTrajectory>::iterator u = other_uav_avoidance_trajectories_.begin();
+    1744             : 
+    1745       68028 :   while (u != other_uav_avoidance_trajectories_.end()) {
+    1746             : 
+    1747             :     // is the other's trajectory fresh enought?
+    1748        3716 :     if ((ros::Time::now() - u->second.stamp).toSec() < _collision_trajectory_timeout_) {
+    1749             : 
+    1750      152356 :       for (int v = 0; v < MPC_HORIZON_LENGTH; v++) {
+    1751             : 
+    1752             :         // check all points of the trajectory for possible collisions
+    1753      148640 :         if (checkCollision(predicted_trajectory_(v * MPC_N_STATES, 0), predicted_trajectory_(v * MPC_N_STATES + 4, 0),
+    1754      148640 :                            predicted_trajectory_(v * MPC_N_STATES + 8, 0), u->second.points[v].x, u->second.points[v].y, u->second.points[v].z)) {
+    1755             : 
+    1756             :           // collision is detected
+    1757       22350 :           int other_uav_priority = INT_MAX;
+    1758             :           // get the priority of the other uav
+    1759             :           /* sscanf(u->first.c_str(), "uav%d", &other_uav_priority); */
+    1760       22350 :           other_uav_priority = u->second.priority;
+    1761             : 
+    1762             :           // check if we should be avoiding (out priority is higher, or the other uav has collision avoidance turned off)
+    1763       22350 :           if ((u->second.collision_avoidance == false) || (other_uav_priority < avoidance_this_uav_priority_)) {
+    1764             : 
+    1765             :             // we should be avoiding
+    1766       14692 :             avoiding_collision_      = true;
+    1767       14692 :             double tmp_safe_altitude = u->second.points[v].z + _avoidance_z_correction_;
+    1768             : 
+    1769       14692 :             if (tmp_safe_altitude > collision_free_altitude_ && v <= _avoidance_collision_start_climbing_) {
+    1770         373 :               collision_free_altitude_ = tmp_safe_altitude;
+    1771             :             }
+    1772             : 
+    1773       14692 :             ROS_ERROR_STREAM_THROTTLE(1, "[MpcTracker]: avoiding collision with uav" << other_uav_priority);
+    1774             : 
+    1775             :           } else {
+    1776             :             // the other uav should avoid us
+    1777        7658 :             ROS_WARN_STREAM_THROTTLE(1, "[MpcTracker]: detected collision with uav" << other_uav_priority << ", not avoiding (my priority is higher)");
+    1778             :           }
+    1779             :         }
+    1780             : 
+    1781      148640 :         if (checkCollisionInflated(predicted_trajectory_(v * MPC_N_STATES, 0), predicted_trajectory_(v * MPC_N_STATES + 4, 0),
+    1782      148640 :                                    predicted_trajectory_(v * MPC_N_STATES + 8, 0), u->second.points[v].x, u->second.points[v].y, u->second.points[v].z)) {
+    1783             : 
+    1784             :           // collision is detected
+    1785      148640 :           if (first_collision_index > v) {
+    1786        3716 :             first_collision_index = v;
+    1787             :           }
+    1788             :         }
+    1789             :       }
+    1790             :     }
+    1791        3716 :     u++;
+    1792             :   }
+    1793             : 
+    1794       64312 :   if (!avoiding_collision_) {
+    1795             : 
+    1796       63457 :     auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    1797             : 
+    1798             :     // we are not avoiding any collisions, so we slowly reduce the collision avoidance offset to return to normal flight
+    1799       63457 :     collision_free_altitude_ -= 2.0 / (1.0 / dt1);
+    1800             : 
+    1801       63457 :     double safety_area_min_z = std::numeric_limits<double>::lowest();
+    1802             : 
+    1803       63457 :     if (collision_free_altitude_ < safety_area_min_z) {
+    1804             : 
+    1805           0 :       collision_free_altitude_ = safety_area_min_z;
+    1806             :     }
+    1807             :   }
+    1808             : 
+    1809      128624 :   return collision_free_altitude_;
+    1810             : }
+    1811             : 
+    1812             : //}
+    1813             : 
+    1814             : // | ------------------ trajectory filtering ------------------ |
+    1815             : 
+    1816             : /* //{ filterReferenceXY() */
+    1817             : 
+    1818       65275 : std::tuple<MatrixXd, MatrixXd> MpcTracker::filterReferenceXY(const VectorXd& des_x_trajectory, const VectorXd& des_y_trajectory, double max_speed_x,
+    1819             :                                                              double max_speed_y) {
+    1820             : 
+    1821       65275 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    1822             : 
+    1823      130550 :   auto mpc_x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1824       65275 :   auto trajectory_dt = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_dt_);
+    1825             : 
+    1826      130550 :   MatrixXd filtered_x_trajectory = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+    1827      130550 :   MatrixXd filtered_y_trajectory = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+    1828             : 
+    1829             :   double difference_x;
+    1830             :   double difference_y;
+    1831             :   double max_sample_x;
+    1832             :   double max_sample_y;
+    1833             : 
+    1834     2676275 :   for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    1835             : 
+    1836     2611000 :     if (i == 0) {
+    1837       65275 :       max_sample_x = max_speed_x * dt1;
+    1838       65275 :       max_sample_y = max_speed_y * dt1;
+    1839       65275 :       difference_x = des_x_trajectory(i, 0) - mpc_x(0, 0);
+    1840       65275 :       difference_y = des_y_trajectory(i, 0) - mpc_x(4, 0);
+    1841             :     } else {
+    1842     2545725 :       max_sample_x = max_speed_x * _dt2_;
+    1843     2545725 :       max_sample_y = max_speed_y * _dt2_;
+    1844     2545725 :       difference_x = des_x_trajectory(i, 0) - filtered_x_trajectory(i - 1, 0);
+    1845     2545725 :       difference_y = des_y_trajectory(i, 0) - filtered_y_trajectory(i - 1, 0);
+    1846             :     }
+    1847             : 
+    1848     2611000 :     if (!trajectory_tracking_in_progress_) {
+    1849             : 
+    1850     2123360 :       double direction_angle  = atan2(difference_y, difference_x);
+    1851     2123360 :       double max_dir_sample_x = abs(max_sample_x * cos(direction_angle));
+    1852     2123360 :       double max_dir_sample_y = abs(max_sample_y * sin(direction_angle));
+    1853             : 
+    1854     2123360 :       if (max_sample_x > max_dir_sample_x) {
+    1855      261388 :         max_sample_x = max_dir_sample_x;
+    1856             :       }
+    1857     2123360 :       if (max_sample_y > max_dir_sample_y) {
+    1858     2122633 :         max_sample_y = max_dir_sample_y;
+    1859             :       }
+    1860             : 
+    1861             :       // saturate the difference
+    1862     2123360 :       if (difference_x > max_sample_x)
+    1863      111968 :         difference_x = max_sample_x;
+    1864     2011392 :       else if (difference_x < -max_sample_x)
+    1865      165356 :         difference_x = -max_sample_x;
+    1866             : 
+    1867     2123360 :       if (difference_y > max_sample_y)
+    1868       57756 :         difference_y = max_sample_y;
+    1869     2065604 :       else if (difference_y < -max_sample_y)
+    1870      198153 :         difference_y = -max_sample_y;
+    1871             :     }
+    1872             : 
+    1873     2611000 :     if (i == 0) {
+    1874       65275 :       filtered_x_trajectory(i, 0) = mpc_x(0, 0) + difference_x;
+    1875       65275 :       filtered_y_trajectory(i, 0) = mpc_x(4, 0) + difference_y;
+    1876             :     } else {
+    1877     2545725 :       filtered_x_trajectory(i, 0) = filtered_x_trajectory(i - 1, 0) + difference_x;
+    1878     2545725 :       filtered_y_trajectory(i, 0) = filtered_y_trajectory(i - 1, 0) + difference_y;
+    1879             :     }
+    1880             :   }
+    1881             : 
+    1882             :   // | ----------------------- add wiggle ----------------------- |
+    1883             : 
+    1884       65275 :   auto [wiggle_enabled, wiggle_amplitude, wiggle_frequency_] =
+    1885       65275 :       mrs_lib::get_mutexed(mutex_drs_params_, drs_params_.wiggle_enabled, drs_params_.wiggle_amplitude, drs_params_.wiggle_frequency);
+    1886             : 
+    1887       65275 :   if (wiggle_enabled) {
+    1888             : 
+    1889           0 :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    1890           0 :       filtered_x_trajectory(i, 0) += wiggle_amplitude * cos(wiggle_frequency_ * 2 * M_PI * i * trajectory_dt + wiggle_phase_);
+    1891           0 :       filtered_y_trajectory(i, 0) += wiggle_amplitude * sin(wiggle_frequency_ * 2 * M_PI * i * trajectory_dt + wiggle_phase_);
+    1892             :     }
+    1893             : 
+    1894           0 :     wiggle_phase_ += wiggle_frequency_ * dt1 * 2 * M_PI;
+    1895             : 
+    1896           0 :     if (wiggle_phase_ > M_PI) {
+    1897           0 :       wiggle_phase_ -= 2 * M_PI;
+    1898             :     }
+    1899             :   }
+    1900             : 
+    1901      130550 :   return std::make_tuple(filtered_x_trajectory, filtered_y_trajectory);
+    1902             : }
+    1903             : 
+    1904             : //}
+    1905             : 
+    1906             : /* //{ filterReferenceZ() */
+    1907             : 
+    1908       65275 : MatrixXd MpcTracker::filterReferenceZ(const VectorXd& des_z_trajectory, const double max_ascending_speed, const double max_descending_speed) {
+    1909             : 
+    1910      130550 :   auto mpc_x = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    1911             : 
+    1912       65275 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    1913             : 
+    1914             :   double difference_z;
+    1915             :   double max_sample_z;
+    1916             : 
+    1917       65275 :   MatrixXd filtered_trajectory = MatrixXd::Zero(MPC_HORIZON_LENGTH, 1);
+    1918             : 
+    1919       65275 :   double current_z = mpc_x(8, 0);
+    1920             : 
+    1921     2676275 :   for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    1922             : 
+    1923     2611000 :     if (i == 0) {
+    1924             : 
+    1925       65275 :       difference_z = des_z_trajectory(i, 0) - current_z;
+    1926             : 
+    1927       65275 :       if (difference_z > 0) {
+    1928       23102 :         max_sample_z = max_ascending_speed * dt1;
+    1929             :       } else {
+    1930       42173 :         max_sample_z = max_descending_speed * dt1;
+    1931             :       }
+    1932             : 
+    1933             :     } else {
+    1934             : 
+    1935     2545725 :       difference_z = des_z_trajectory(i, 0) - filtered_trajectory(i - 1, 0);
+    1936             : 
+    1937     2545725 :       if (difference_z > 0) {
+    1938      177081 :         max_sample_z = max_ascending_speed * _dt2_;
+    1939             :       } else {
+    1940     2368644 :         max_sample_z = max_descending_speed * _dt2_;
+    1941             :       }
+    1942             :     }
+    1943             : 
+    1944     2611000 :     if (!trajectory_tracking_in_progress_) {
+    1945             : 
+    1946             :       // saturate the difference
+    1947     2123400 :       if (difference_z > max_sample_z)
+    1948       77575 :         difference_z = max_sample_z;
+    1949     2045825 :       else if (difference_z < -max_sample_z)
+    1950       19911 :         difference_z = -max_sample_z;
+    1951             :     }
+    1952             : 
+    1953     2611000 :     if (i == 0) {
+    1954       65275 :       filtered_trajectory(i, 0) = current_z + difference_z;
+    1955             :     } else {
+    1956     2545725 :       filtered_trajectory(i, 0) = filtered_trajectory(i - 1, 0) + difference_z;
+    1957             :     }
+    1958             :   }
+    1959             : 
+    1960      130550 :   return filtered_trajectory;
+    1961             : }
+    1962             : 
+    1963             : //}
+    1964             : 
+    1965             : /* //{ manageConstraints() */
+    1966             : 
+    1967       65275 : void MpcTracker::manageConstraints() {
+    1968             : 
+    1969       65275 :   if (!got_constraints_) {
+    1970       65149 :     return;
+    1971             :   }
+    1972             : 
+    1973       65275 :   if (all_constraints_set_) {
+    1974       65149 :     return;
+    1975             :   }
+    1976             : 
+    1977         126 :   auto constraints            = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    1978         252 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    1979             : 
+    1980         252 :   bool can_change = (fabs(mpc_x(1, 0)) < constraints.horizontal_speed) && (fabs(mpc_x(2, 0)) < constraints.horizontal_acceleration) &&
+    1981         126 :                     (fabs(mpc_x(3, 0)) < constraints.horizontal_jerk) && (fabs(mpc_x(5, 0)) < constraints.horizontal_speed) &&
+    1982         126 :                     (fabs(mpc_x(6, 0)) < constraints.horizontal_acceleration) && (fabs(mpc_x(7, 0)) < constraints.horizontal_jerk) &&
+    1983         126 :                     (mpc_x(9, 0) < constraints.vertical_ascending_speed) && (mpc_x(9, 0) > -constraints.vertical_descending_speed) &&
+    1984         126 :                     (mpc_x(10, 0) < constraints.vertical_ascending_acceleration) && (mpc_x(10, 0) > -constraints.vertical_descending_acceleration) &&
+    1985         126 :                     (mpc_x(11, 0) < constraints.vertical_ascending_jerk) && (mpc_x(11, 0) > -constraints.vertical_descending_jerk) &&
+    1986         378 :                     (fabs(mpc_x_heading(1, 0)) < constraints.heading_speed) && (fabs(mpc_x_heading(2, 0)) < constraints.heading_acceleration) &&
+    1987         126 :                     (fabs(mpc_x_heading(3, 0)) < constraints.heading_jerk);
+    1988             : 
+    1989         126 :   if (can_change) {
+    1990             : 
+    1991             :     {
+    1992         126 :       std::scoped_lock lock(mutex_constraints_filtered_);
+    1993             : 
+    1994         126 :       constraints_filtered_.horizontal_acceleration = constraints.horizontal_acceleration;
+    1995         126 :       constraints_filtered_.horizontal_jerk         = constraints.horizontal_jerk;
+    1996         126 :       constraints_filtered_.horizontal_snap         = constraints.horizontal_snap;
+    1997             : 
+    1998         126 :       constraints_filtered_.vertical_ascending_acceleration = constraints.vertical_ascending_acceleration;
+    1999         126 :       constraints_filtered_.vertical_ascending_jerk         = constraints.vertical_ascending_jerk;
+    2000         126 :       constraints_filtered_.vertical_ascending_snap         = constraints.vertical_ascending_snap;
+    2001             : 
+    2002         126 :       constraints_filtered_.vertical_descending_acceleration = constraints.vertical_descending_acceleration;
+    2003         126 :       constraints_filtered_.vertical_descending_jerk         = constraints.vertical_descending_jerk;
+    2004         126 :       constraints_filtered_.vertical_descending_snap         = constraints.vertical_descending_snap;
+    2005             : 
+    2006         126 :       constraints_filtered_.heading_acceleration = constraints.heading_acceleration;
+    2007         126 :       constraints_filtered_.heading_jerk         = constraints.heading_jerk;
+    2008         126 :       constraints_filtered_.heading_snap         = constraints.heading_snap;
+    2009             :     }
+    2010             : 
+    2011         126 :     ROS_INFO_THROTTLE(1.0, "[MpcTracker]: all constraints succesfully applied");
+    2012         126 :     all_constraints_set_ = true;
+    2013             : 
+    2014             :   } else {
+    2015           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: slowing down to apply new constraints");
+    2016             :   }
+    2017             : }
+    2018             : 
+    2019             : //}
+    2020             : 
+    2021             : /* //{ calculateMPC() */
+    2022             : 
+    2023       65275 : void MpcTracker::calculateMPC() {
+    2024             : 
+    2025       65275 :   std::scoped_lock lock(mutex_mpc_calculation_);
+    2026             : 
+    2027       65275 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2028             : 
+    2029       65275 :   ROS_DEBUG_STREAM_THROTTLE(1.0, "[MpcTracker]: MPC calculation dt = " << dt1);
+    2030             : 
+    2031       65275 :   auto constraints            = mrs_lib::get_mutexed(mutex_constraints_filtered_, constraints_filtered_);
+    2032       65275 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    2033       65275 :   auto uav_state              = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2034       65275 :   auto drs_params             = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+    2035             : 
+    2036       65275 :   MatrixXd des_x_trajectory, des_y_trajectory, des_z_trajectory, des_heading_trajectory;
+    2037             :   {
+    2038      130550 :     std::scoped_lock lock(mutex_des_trajectory_);
+    2039             : 
+    2040       65275 :     des_x_trajectory       = des_x_trajectory_;
+    2041       65275 :     des_y_trajectory       = des_y_trajectory_;
+    2042       65275 :     des_z_trajectory       = des_z_trajectory_;
+    2043       65275 :     des_heading_trajectory = des_heading_trajectory_;
+    2044             :   }
+    2045             : 
+    2046       65275 :   int    first_collision_index = INT_MAX;
+    2047       65275 :   double lowest_z              = std::numeric_limits<double>::max();
+    2048             : 
+    2049       65275 :   if (collision_avoidance_enabled_ && (uav_state.estimator_horizontal == "lat_gps" || uav_state.estimator_horizontal == "lat_rtk")) {
+    2050             : 
+    2051             :     // determine the lowest point in our trajectory
+    2052     2636792 :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2053     2572480 :       if (des_z_trajectory_(i, 0) < lowest_z) {
+    2054       84826 :         lowest_z = des_z_trajectory_(i, 0);
+    2055             :       }
+    2056             :     }
+    2057             : 
+    2058             :     // check other drone trajectories for collisions
+    2059       64312 :     minimum_collison_free_altitude_ = checkTrajectoryForCollisions(first_collision_index);
+    2060             : 
+    2061             :   } else {
+    2062             : 
+    2063         963 :     minimum_collison_free_altitude_ = std::numeric_limits<double>::lowest();
+    2064             :   }
+    2065             : 
+    2066       65275 :   double max_speed_x = constraints.horizontal_speed;
+    2067       65275 :   double max_speed_y = constraints.horizontal_speed;
+    2068       65275 :   double max_speed_z = constraints.vertical_ascending_speed;
+    2069       65275 :   double min_speed_z = constraints.vertical_descending_speed;
+    2070             : 
+    2071       65275 :   double max_acc_x = constraints.horizontal_acceleration;
+    2072       65275 :   double max_acc_y = constraints.horizontal_acceleration;
+    2073       65275 :   double max_acc_z = constraints.vertical_ascending_acceleration;
+    2074       65275 :   double min_acc_z = constraints.vertical_descending_acceleration;
+    2075             : 
+    2076       65275 :   double max_snap_x = constraints.horizontal_snap;
+    2077       65275 :   double max_snap_y = constraints.horizontal_snap;
+    2078       65275 :   double max_snap_z = constraints.vertical_ascending_snap;
+    2079       65275 :   double min_snap_z = constraints.vertical_descending_snap;
+    2080             : 
+    2081       65275 :   double max_jerk_x = constraints.horizontal_jerk;
+    2082       65275 :   double max_jerk_y = constraints.horizontal_jerk;
+    2083       65275 :   double max_jerk_z = constraints.vertical_ascending_jerk;
+    2084       65275 :   double min_jerk_z = constraints.vertical_descending_jerk;
+    2085             : 
+    2086       65275 :   collision_avoidance_affecting_me_ = false;
+    2087             : 
+    2088       65275 :   if (first_collision_index < MPC_HORIZON_LENGTH) {
+    2089             : 
+    2090        3716 :     collision_avoidance_affecting_me_ = true;
+    2091             :     // the tmp variable is used to scale the speed of our drone in collision avoidance, depending on how far away the collision is
+    2092        3716 :     double tmp = 0;
+    2093             : 
+    2094        3716 :     if (first_collision_index <= _avoidance_collision_slow_down_fully_) {
+    2095        3716 :       tmp = 1;
+    2096           0 :     } else if (first_collision_index <= _avoidance_collision_slow_down_) {
+    2097           0 :       tmp = 1.0 - ((double)(first_collision_index - _avoidance_collision_slow_down_fully_)) /
+    2098           0 :                       (double)(_avoidance_collision_slow_down_ - _avoidance_collision_slow_down_fully_);
+    2099           0 :       tmp = tmp * tmp;
+    2100             :     }
+    2101             : 
+    2102        3716 :     if (!std::isfinite(tmp)) {
+    2103           0 :       tmp = 1.0;
+    2104           0 :       ROS_ERROR("[MpcTracker]: NaN detected in variable 'tmp', setting it to 1.0 and returning!!!");
+    2105           0 :       return;
+    2106        3716 :     } else if (tmp > 1.0) {
+    2107           0 :       tmp = 1.0;
+    2108        3716 :     } else if (tmp < 0.0) {
+    2109           0 :       tmp = 0.0;
+    2110             :     }
+    2111             : 
+    2112        3716 :     if (tmp > coef_scaler) {
+    2113           6 :       coef_scaler = tmp;
+    2114           6 :       coef_time   = ros::Time::now();
+    2115             :     }
+    2116        3716 :     if ((ros::Time::now() - coef_time).toSec() > 2.0) {
+    2117        3264 :       coef_scaler = tmp;
+    2118             :     }
+    2119             : 
+    2120             :     // we are close to a possible collision, better slow down a bit to give everyone more time
+    2121        3716 :     max_speed_x = constraints.horizontal_speed * ((_avoidance_collision_horizontal_speed_coef_ * coef_scaler) + (1.0 - coef_scaler));
+    2122        3716 :     max_speed_y = constraints.horizontal_speed * ((_avoidance_collision_horizontal_speed_coef_ * coef_scaler) + (1.0 - coef_scaler));
+    2123             :   }
+    2124             : 
+    2125       65275 :   if (collision_free_altitude_ > lowest_z) {
+    2126             : 
+    2127        1976 :     collision_avoidance_affecting_me_ = true;
+    2128        1976 :     max_speed_x                       = constraints.horizontal_speed * (_avoidance_collision_horizontal_speed_coef_);
+    2129        1976 :     max_speed_y                       = constraints.horizontal_speed * (_avoidance_collision_horizontal_speed_coef_);
+    2130             :   }
+    2131             : 
+    2132             :   // first control input generated by MPC
+    2133      130550 :   VectorXd mpc_u         = VectorXd::Zero(MPC_N_INPUTS);
+    2134       65275 :   double   mpc_u_heading = 0;
+    2135             : 
+    2136       65275 :   double iters_z       = 0;
+    2137       65275 :   double iters_x       = 0;
+    2138       65275 :   double iters_y       = 0;
+    2139       65275 :   double iters_heading = 0;
+    2140             : 
+    2141       65275 :   ros::Time time_begin = ros::Time::now();
+    2142             : 
+    2143      130550 :   MatrixXd des_z_filtered = filterReferenceZ(des_z_trajectory, max_speed_z, min_speed_z);
+    2144             : 
+    2145     2676275 :   for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2146     2611000 :     if (des_z_filtered(i, 0) < minimum_collison_free_altitude_) {
+    2147       78487 :       des_z_filtered_offset_(i, 0) = minimum_collison_free_altitude_;
+    2148             :     } else {
+    2149     2532513 :       des_z_filtered_offset_(i, 0) = des_z_filtered(i, 0);
+    2150             :     }
+    2151             :   }
+    2152             : 
+    2153             :   // | ----------------- prepare the references ----------------- |
+    2154             : 
+    2155             :   // | -------------------- MPC solver z-axis ------------------- |
+    2156             : 
+    2157       65275 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2158       50486 :     mpc_solver_z_->setVelQ(drs_params.q_vel_braking);
+    2159             :   } else {
+    2160       14789 :     mpc_solver_z_->setVelQ(drs_params.q_vel_no_braking);
+    2161             :   }
+    2162             : 
+    2163      130550 :   MatrixXd initial_z = MatrixXd::Zero(4, 1);
+    2164             : 
+    2165       65275 :   initial_z(0, 0) = mpc_x(8, 0);
+    2166       65275 :   initial_z(1, 0) = mpc_x(9, 0);
+    2167       65275 :   initial_z(2, 0) = mpc_x(10, 0);
+    2168       65275 :   initial_z(3, 0) = mpc_x(11, 0);
+    2169             : 
+    2170       65275 :   mpc_solver_z_->setDt(dt1);
+    2171       65275 :   mpc_solver_z_->setInitialState(initial_z);
+    2172       65275 :   mpc_solver_z_->loadReference(des_z_filtered_offset_);
+    2173       65275 :   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);
+    2174       65275 :   iters_z += mpc_solver_z_->solveMPC();
+    2175             : 
+    2176             :   {
+    2177      130550 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2178             : 
+    2179       65275 :     mpc_solver_z_->getStates(predicted_trajectory_);
+    2180             :   }
+    2181             : 
+    2182       65275 :   mpc_u(2) = mpc_solver_z_->getFirstControlInput();
+    2183             : 
+    2184             :   // if we are climbing to avoid a collision, reduce or arrest our horizontal velocity
+    2185             :   double ascend;
+    2186             :   {
+    2187       65275 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2188             : 
+    2189       65275 :     ascend = (predicted_trajectory_(10, 0) / max_speed_z);
+    2190             :   }
+    2191             : 
+    2192       65275 :   if (ascend > 0 && collision_free_altitude_ > lowest_z) {
+    2193        1028 :     max_speed_y = max_speed_y * (1.0 - ascend);
+    2194        1028 :     max_speed_x = max_speed_x * (1.0 - ascend);
+    2195             :   }
+    2196             : 
+    2197      195825 :   auto [des_x_filtered, des_y_filtered] = filterReferenceXY(des_x_trajectory, des_y_trajectory, max_speed_x, max_speed_y);
+    2198             : 
+    2199             :   // unwrap the heading reference
+    2200             : 
+    2201       65275 :   des_heading_trajectory(0, 0) = sradians::unwrap(des_heading_trajectory(0, 0), mpc_x_heading(0));
+    2202             : 
+    2203     2611000 :   for (int i = 1; i < MPC_HORIZON_LENGTH; i++) {
+    2204     2545725 :     des_heading_trajectory(i, 0) = sradians::unwrap(des_heading_trajectory(i, 0), des_heading_trajectory(i - 1, 0));
+    2205             :   }
+    2206             : 
+    2207             :   // | -------------------- MPC solver x-axis ------------------- |
+    2208             : 
+    2209       65275 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2210       50486 :     mpc_solver_x_->setVelQ(drs_params.q_vel_braking);
+    2211             :   } else {
+    2212       14789 :     mpc_solver_x_->setVelQ(drs_params.q_vel_no_braking);
+    2213             :   }
+    2214             : 
+    2215      130550 :   MatrixXd initial_x = MatrixXd::Zero(4, 1);
+    2216             : 
+    2217       65275 :   initial_x(0, 0) = mpc_x(0, 0);
+    2218       65275 :   initial_x(1, 0) = mpc_x(1, 0);
+    2219       65275 :   initial_x(2, 0) = mpc_x(2, 0);
+    2220       65275 :   initial_x(3, 0) = mpc_x(3, 0);
+    2221             : 
+    2222       65275 :   mpc_solver_x_->setDt(dt1);
+    2223       65275 :   mpc_solver_x_->setInitialState(initial_x);
+    2224       65275 :   mpc_solver_x_->loadReference(des_x_filtered);
+    2225       65275 :   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);
+    2226       65275 :   iters_x += mpc_solver_x_->solveMPC();
+    2227             : 
+    2228             :   {
+    2229      130550 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2230             : 
+    2231       65275 :     mpc_solver_x_->getStates(predicted_trajectory_);
+    2232             :   }
+    2233             : 
+    2234       65275 :   mpc_u(0) = mpc_solver_x_->getFirstControlInput();
+    2235             : 
+    2236             :   // | -------------------- MPC solver y-axis ------------------- |
+    2237             : 
+    2238       65275 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2239       50484 :     mpc_solver_y_->setVelQ(drs_params.q_vel_braking);
+    2240             :   } else {
+    2241       14791 :     mpc_solver_y_->setVelQ(drs_params.q_vel_no_braking);
+    2242             :   }
+    2243             : 
+    2244      130550 :   MatrixXd initial_y = MatrixXd::Zero(4, 1);
+    2245             : 
+    2246       65275 :   initial_y(0, 0) = mpc_x(4, 0);
+    2247       65275 :   initial_y(1, 0) = mpc_x(5, 0);
+    2248       65275 :   initial_y(2, 0) = mpc_x(6, 0);
+    2249       65275 :   initial_y(3, 0) = mpc_x(7, 0);
+    2250             : 
+    2251       65275 :   mpc_solver_y_->setDt(dt1);
+    2252       65275 :   mpc_solver_y_->setInitialState(initial_y);
+    2253       65275 :   mpc_solver_y_->loadReference(des_y_filtered);
+    2254       65275 :   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);
+    2255       65275 :   iters_y += mpc_solver_y_->solveMPC();
+    2256             :   {
+    2257      130550 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2258             : 
+    2259       65275 :     mpc_solver_y_->getStates(predicted_trajectory_);
+    2260             :   }
+    2261       65275 :   mpc_u(1) = mpc_solver_y_->getFirstControlInput();
+    2262             : 
+    2263             :   // | ------------------- MPC solver heading ------------------- |
+    2264             : 
+    2265       65275 :   if (brake_ && !trajectory_tracking_in_progress_) {
+    2266       50484 :     mpc_solver_heading_->setVelQ(drs_params.q_vel_braking);
+    2267             :   } else {
+    2268       14791 :     mpc_solver_heading_->setVelQ(drs_params.q_vel_no_braking);
+    2269             :   }
+    2270             : 
+    2271       65275 :   mpc_solver_heading_->setDt(dt1);
+    2272       65275 :   mpc_solver_heading_->setInitialState(mpc_x_heading);
+    2273       65275 :   mpc_solver_heading_->loadReference(des_heading_trajectory);
+    2274       65275 :   mpc_solver_heading_->setLimits(constraints.heading_speed, constraints.heading_speed, constraints.heading_acceleration, constraints.heading_acceleration,
+    2275             :                                  constraints.heading_jerk, constraints.heading_jerk, constraints.heading_snap, constraints.heading_snap);
+    2276       65275 :   iters_heading += mpc_solver_heading_->solveMPC();
+    2277             :   {
+    2278      130550 :     std::scoped_lock lock(mutex_predicted_trajectory_);
+    2279             : 
+    2280       65275 :     mpc_solver_heading_->getStates(predicted_heading_trajectory_);
+    2281             :   }
+    2282       65275 :   mpc_u_heading = mpc_solver_heading_->getFirstControlInput();
+    2283             : 
+    2284             :   {
+    2285       65275 :     bool saturating = false;
+    2286             : 
+    2287       65275 :     if (mpc_u(0) > max_snap_x * 1.01) {
+    2288           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap X: " << mpc_u(0));
+    2289           0 :       mpc_u(0)   = max_snap_x;
+    2290           0 :       saturating = true;
+    2291             :     }
+    2292       65275 :     if (mpc_u(0) < -max_snap_x * 1.01) {
+    2293           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap X: " << mpc_u(0));
+    2294           0 :       mpc_u(0)   = -max_snap_x;
+    2295           0 :       saturating = true;
+    2296             :     }
+    2297       65275 :     if (mpc_u(1) > max_snap_y * 1.01) {
+    2298           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Y: " << mpc_u(1));
+    2299           0 :       mpc_u(1)   = max_snap_y;
+    2300           0 :       saturating = true;
+    2301             :     }
+    2302       65275 :     if (mpc_u(1) < -max_snap_y * 1.01) {
+    2303           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Y: " << mpc_u(1));
+    2304           0 :       mpc_u(1)   = -max_snap_y;
+    2305           0 :       saturating = true;
+    2306             :     }
+    2307       65275 :     if (mpc_u(2) > max_snap_z * 1.01) {
+    2308           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Z: " << mpc_u(2));
+    2309           0 :       mpc_u(2)   = max_snap_z;
+    2310           0 :       saturating = true;
+    2311             :     }
+    2312       65275 :     if (mpc_u(2) < -min_snap_z * 1.01) {
+    2313           0 :       ROS_WARN_STREAM_THROTTLE(0.1, "[MpcTracker]: saturating snap Z: " << mpc_u(2));
+    2314           0 :       mpc_u(2)   = -min_snap_z;
+    2315           0 :       saturating = true;
+    2316             :     }
+    2317             : 
+    2318       65275 :     if (saturating) {
+    2319           0 :       debugPrintState(0.1);
+    2320           0 :       debugPrintMPCResult(0.1);
+    2321             :     }
+    2322             :   }
+    2323             : 
+    2324             :   {
+    2325       65275 :     std::scoped_lock lock(mutex_mpc_u_);
+    2326             : 
+    2327       65275 :     mpc_u_         = mpc_u;
+    2328       65275 :     mpc_u_heading_ = mpc_u_heading;
+    2329             :   }
+    2330             : 
+    2331       65275 :   double mpc_solver_time = (ros::Time::now() - time_begin).toSec();
+    2332       65275 :   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_) {
+    2333           0 :     ROS_DEBUG_STREAM_THROTTLE(1.0, "[MpcTracker]: Total MPC solver time: " << mpc_solver_time << " iters X: " << iters_x << "/" << _max_iters_xy_
+    2334             :                                                                            << " iters Y:  " << iters_y << "/" << _max_iters_xy_ << " iters Z: " << iters_z
+    2335             :                                                                            << "/" << _max_iters_z_ << " iters heading: " << iters_heading << "/"
+    2336             :                                                                            << _max_iters_heading_);
+    2337             :   }
+    2338             : 
+    2339       65275 :   future_was_predicted_ = true;
+    2340             : 
+    2341             :   // | ------------- breaking for the next iteration ------------ |
+    2342             : 
+    2343      124524 :   if (drs_params.braking_enabled && (std::abs(des_x_filtered(MPC_HORIZON_LENGTH - 6) - des_x_filtered(MPC_HORIZON_LENGTH - 1)) <= 0.1) &&
+    2344      114104 :       (std::abs(des_y_filtered(MPC_HORIZON_LENGTH - 6) - des_y_filtered(MPC_HORIZON_LENGTH - 1)) <= 0.1) &&
+    2345      185405 :       (std::abs(des_z_filtered(MPC_HORIZON_LENGTH - 6) - des_z_filtered(MPC_HORIZON_LENGTH - 1)) <= 0.1) &&
+    2346       53322 :       (std::abs(radians::diff(des_heading_trajectory(MPC_HORIZON_LENGTH - 6), des_heading_trajectory(MPC_HORIZON_LENGTH - 1))) <= 0.1)) {
+    2347             : 
+    2348       53193 :     brake_ = true;
+    2349       53193 :     ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: braking");
+    2350             : 
+    2351             :   } else {
+    2352       12082 :     brake_ = false;
+    2353             :   }
+    2354             : 
+    2355             :   /* publish mpc reference //{ */
+    2356             : 
+    2357             :   {
+    2358      130550 :     geometry_msgs::PoseArray debug_trajectory_out;
+    2359       65275 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    2360       65275 :     debug_trajectory_out.header.frame_id = uav_state_.header.frame_id;
+    2361             : 
+    2362             :     {
+    2363      130550 :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    2364             : 
+    2365     2676275 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2366             : 
+    2367     2611000 :         geometry_msgs::Pose new_pose;
+    2368             : 
+    2369     2611000 :         new_pose.position.x = des_x_filtered(i, 0);
+    2370     2611000 :         new_pose.position.y = des_y_filtered(i, 0);
+    2371     2611000 :         new_pose.position.z = des_z_filtered(i, 0);
+    2372             : 
+    2373     2611000 :         new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, des_heading_trajectory(i));
+    2374             : 
+    2375     2611000 :         debug_trajectory_out.poses.push_back(new_pose);
+    2376             :       }
+    2377             :     }
+    2378             : 
+    2379       65275 :     ph_mpc_reference_debugging_.publish(debug_trajectory_out);
+    2380             :   }
+    2381             : 
+    2382             :   //}
+    2383             : }
+    2384             : 
+    2385             : //}
+    2386             : 
+    2387             : /* iterateModel() //{ */
+    2388             : 
+    2389       59539 : void MpcTracker::iterateModel(const double& dt) {
+    2390             : 
+    2391       59539 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2392             : 
+    2393       59539 :   if (model_first_iteration_) {
+    2394             : 
+    2395          75 :     model_iteration_last_time_ = ros::Time::now();
+    2396          75 :     model_first_iteration_     = false;
+    2397             : 
+    2398             :   } else {
+    2399             : 
+    2400       59464 :     dt1 = 0.9 * dt1 + 0.1 * dt;
+    2401             : 
+    2402       59464 :     mrs_lib::set_mutexed(mutex_dt1_, dt1, dt1_);
+    2403       59464 :     timer_mpc_iteration_.setPeriod(ros::Duration(dt1), false);
+    2404             : 
+    2405             :     // clang-format off
+    2406       59464 :     A_ << 1, dt1, 0.5*dt1*dt1, 0,           0, 0,   0,           0,           0, 0,   0,           0,
+    2407       59464 :           0, 1,   dt1,         0.5*dt1*dt1, 0, 0,   0,           0,           0, 0,   0,           0,
+    2408       59464 :           0, 0,   1,           dt1,         0, 0,   0,           0,           0, 0,   0,           0,
+    2409       59464 :           0, 0,   0,           1,           0, 0,   0,           0,           0, 0,   0,           0,
+    2410       59464 :           0, 0,   0,           0,           1, dt1, 0.5*dt1*dt1, 0,           0, 0,   0,           0,
+    2411       59464 :           0, 0,   0,           0,           0, 1,   dt1,         0.5*dt1*dt1, 0, 0,   0,           0,
+    2412       59464 :           0, 0,   0,           0,           0, 0,   1,           dt1,         0, 0,   0,           0,
+    2413       59464 :           0, 0,   0,           0,           0, 0,   0,           1,           0, 0,   0,           0,
+    2414       59464 :           0, 0,   0,           0,           0, 0,   0,           0,           1, dt1, 0.5*dt1*dt1, 0,
+    2415       59464 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 1,   dt1,         0.5*dt1*dt1,
+    2416       59464 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 0,   1,           dt1,
+    2417       59464 :           0, 0,   0,           0,           0, 0,   0,           0,           0, 0,   0,           1;
+    2418             : 
+    2419       59464 :       B_ << 0,   0,   0,
+    2420       59464 :             0,   0,   0,
+    2421       59464 :             0,   0,   0,
+    2422       59464 :             dt1, 0,   0,
+    2423       59464 :             0,   0,   0,
+    2424       59464 :             0,   0,   0,
+    2425       59464 :             0,   0,   0,
+    2426       59464 :             0,   dt1, 0,
+    2427       59464 :             0,   0,   0,
+    2428       59464 :             0,   0,   0,
+    2429       59464 :             0,   0,   0,
+    2430       59464 :             0,   0,   dt1;
+    2431             : 
+    2432       59464 :       A_heading_ << 1, dt1, 0.5*dt1*dt1, 0,
+    2433       59464 :                     0, 1,   dt1,         0.5*dt1*dt1,
+    2434       59464 :                     0, 0,   1,           dt1,
+    2435       59464 :                     0, 0,   0,           1;
+    2436             : 
+    2437       59464 :       B_heading_ << 0,
+    2438       59464 :                     0,
+    2439      118928 :                     0,
+    2440       59464 :                     dt1;
+    2441             : 
+    2442       59464 :     model_iteration_last_time_ = ros::Time::now();
+    2443             :   }
+    2444             : 
+    2445             :   {
+    2446      119078 :     auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    2447      119078 :     auto [mpc_u, mpc_u_heading] = mrs_lib::get_mutexed(mutex_mpc_u_, mpc_u_, mpc_u_heading_);
+    2448             : 
+    2449      119078 :     MatrixXd new_mpc_x         = A_ * mpc_x + B_ * mpc_u;
+    2450      119078 :     MatrixXd new_mpc_x_heading = A_heading_ * mpc_x_heading + B_heading_ * mpc_u_heading;
+    2451             : 
+    2452             :     // | --------------- check the state difference --------------- |
+    2453             :     {
+    2454       59539 :       auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    2455             : 
+    2456       59539 :       bool problem = false;
+    2457             : 
+    2458             :       // position
+    2459             : 
+    2460       59539 :       if (fabs((new_mpc_x(0) - mpc_x(0)) / dt1) > 1.05 * constraints.horizontal_speed) {
+    2461           0 :         ROS_DEBUG("[MpcTracker]: horizontal pos x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(0), new_mpc_x(0),
+    2462             :                   fabs((new_mpc_x(0) - mpc_x(0)) / dt1), constraints.horizontal_speed);
+    2463           0 :         problem = true;
+    2464             :       }
+    2465             : 
+    2466       59539 :       if (fabs((new_mpc_x(4) - mpc_x(4)) / dt1) > 1.05 * constraints.horizontal_speed) {
+    2467           0 :         ROS_DEBUG("[MpcTracker]: horizontal pos y update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(4), new_mpc_x(4),
+    2468             :                   fabs((new_mpc_x(4) - mpc_x(4)) / dt1), constraints.horizontal_speed);
+    2469           0 :         problem = true;
+    2470             :     }
+    2471             : 
+    2472       59539 :       if (((new_mpc_x(8) - mpc_x(8)) / dt1) > 1.05 * constraints.vertical_ascending_speed) {
+    2473           0 :         ROS_DEBUG("[MpcTracker]: vertical pos z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(8), new_mpc_x(8),
+    2474             :                   ((new_mpc_x(8) - mpc_x(8)) / dt1), constraints.vertical_ascending_speed);
+    2475           0 :         problem = true;
+    2476             :       }
+    2477             : 
+    2478       59539 :       if (((new_mpc_x(8) - mpc_x(8)) / dt1) < 1.05 * -constraints.vertical_descending_speed) {
+    2479           0 :         ROS_DEBUG("[MpcTracker]: vertical pos z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(8), new_mpc_x(8),
+    2480             :                   ((new_mpc_x(8) - mpc_x(8)) / dt1), -constraints.vertical_descending_speed);
+    2481           0 :         problem = true;
+    2482             :       }
+    2483             : 
+    2484             :       /* if (fabs(radians::diff(new_mpc_x_heading(0), mpc_x_heading(0)) / dt1) > 1.2 * constraints.heading_speed) { */
+    2485             :       /*   ROS_DEBUG("[MpcTracker]: heading update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x_heading(0), new_mpc_x_heading(0), */
+    2486             :       /*             fabs(radians::diff(new_mpc_x_heading(0), mpc_x_heading(0)) / dt1), constraints.heading_speed); */
+    2487             :       /*   problem = true; */
+    2488             :       /* } */
+    2489             : 
+    2490             :       // velocity
+    2491             : 
+    2492       59539 :       if (fabs((new_mpc_x(1) - mpc_x(1)) / dt1) > 1.05 * constraints.horizontal_acceleration) {
+    2493           0 :         ROS_DEBUG("[MpcTracker]: horizontal vel x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(1), new_mpc_x(1),
+    2494             :                   fabs((new_mpc_x(1) - mpc_x(1)) / dt1), constraints.horizontal_acceleration);
+    2495           0 :         problem = true;
+    2496             :       }
+    2497             : 
+    2498       59539 :       if (fabs((new_mpc_x(5) - mpc_x(5)) / dt1) > 1.05 * constraints.horizontal_acceleration) {
+    2499           0 :         ROS_DEBUG("[MpcTracker]: horizontal vel y update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(5), new_mpc_x(5),
+    2500             :                   fabs((new_mpc_x(5) - mpc_x(5)) / dt1), constraints.horizontal_acceleration);
+    2501           0 :         problem = true;
+    2502             :       }
+    2503             : 
+    2504       59539 :       if (((new_mpc_x(9) - mpc_x(9)) / dt1) > 1.05 * constraints.vertical_ascending_acceleration) {
+    2505           0 :         ROS_DEBUG("[MpcTracker]: vertical vel z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(9), new_mpc_x(9),
+    2506             :                   ((new_mpc_x(9) - mpc_x(9)) / dt1), constraints.vertical_ascending_acceleration);
+    2507           0 :         problem = true;
+    2508             :       }
+    2509             : 
+    2510       59539 :       if (((new_mpc_x(9) - mpc_x(9)) / dt1) < 1.05 * -constraints.vertical_descending_acceleration) {
+    2511           0 :         ROS_DEBUG("[MpcTracker]: vertical vel z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(9), new_mpc_x(9),
+    2512             :                   ((new_mpc_x(9) - mpc_x(9)) / dt1), -constraints.vertical_descending_acceleration);
+    2513           0 :         problem = true;
+    2514             :       }
+    2515             : 
+    2516             :       // acceleration
+    2517             : 
+    2518       59539 :       if (fabs((new_mpc_x(2) - mpc_x(2)) / dt1) > 1.05 * constraints.horizontal_jerk) {
+    2519           0 :         ROS_DEBUG("[MpcTracker]: horizontal acc x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(2), new_mpc_x(2),
+    2520             :                   fabs((new_mpc_x(2) - mpc_x(2)) / dt1), constraints.horizontal_jerk);
+    2521           0 :         problem = true;
+    2522             :       }
+    2523             : 
+    2524       59539 :       if (fabs((new_mpc_x(6) - mpc_x(6)) / dt1) > 1.05 * constraints.horizontal_jerk) {
+    2525           0 :         ROS_DEBUG("[MpcTracker]: horizontal acc y update violates constraints: %.2f -> %.2f, = %.2f > %.2f", mpc_x(6), new_mpc_x(6),
+    2526             :                   fabs((new_mpc_x(6) - mpc_x(6)) / dt1), constraints.horizontal_jerk);
+    2527           0 :         problem = true;
+    2528             :       }
+    2529             : 
+    2530       59539 :       if (((new_mpc_x(10) - mpc_x(10)) / dt1) > 1.05 * constraints.vertical_ascending_jerk) {
+    2531           0 :         ROS_DEBUG("[MpcTracker]: vertical acc z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(10), new_mpc_x(10),
+    2532             :                   ((new_mpc_x(10) - mpc_x(10)) / dt1), constraints.vertical_ascending_jerk);
+    2533           0 :         problem = true;
+    2534             :       }
+    2535             : 
+    2536       59539 :       if (((new_mpc_x(10) - mpc_x(10)) / dt1) < 1.05 * -constraints.vertical_descending_jerk) {
+    2537           0 :         ROS_DEBUG("[MpcTracker]: vertical acc z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(10), new_mpc_x(10),
+    2538             :                   ((new_mpc_x(10) - mpc_x(10)) / dt1), -constraints.vertical_descending_jerk);
+    2539           0 :         problem = true;
+    2540             :       }
+    2541             : 
+    2542             :       // jerk
+    2543             : 
+    2544       59539 :       if (fabs((new_mpc_x(3) - mpc_x(3)) / dt1) > 1.05 * constraints.horizontal_snap) {
+    2545           0 :         ROS_DEBUG("[MpcTracker]: horizontal jerk x update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(3), new_mpc_x(3),
+    2546             :                   fabs((new_mpc_x(3) - mpc_x(3)) / dt1), constraints.horizontal_snap);
+    2547           0 :         problem = true;
+    2548             :       }
+    2549             : 
+    2550       59539 :       if (fabs((new_mpc_x(7) - mpc_x(7)) / dt1) > 1.05 * constraints.horizontal_snap) {
+    2551           0 :         ROS_DEBUG("[MpcTracker]: horizontal jerk y update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(7), new_mpc_x(7),
+    2552             :                   fabs((new_mpc_x(7) - mpc_x(7)) / dt1), constraints.horizontal_snap);
+    2553           0 :         problem = true;
+    2554             :       }
+    2555             : 
+    2556       59539 :       if (((new_mpc_x(11) - mpc_x(11)) / dt1) > 1.05 * constraints.vertical_ascending_snap) {
+    2557           0 :         ROS_DEBUG("[MpcTracker]: vertical jerk z update violates constraints: %.2f -> %.2f = %.2f, > %.2f", mpc_x(11), new_mpc_x(11),
+    2558             :                   ((new_mpc_x(11) - mpc_x(11)) / dt1), constraints.vertical_ascending_snap);
+    2559           0 :         problem = true;
+    2560             :       }
+    2561             : 
+    2562       59539 :       if (((new_mpc_x(11) - mpc_x(11)) / dt1) < 1.05 * -constraints.vertical_descending_snap) {
+    2563           0 :         ROS_DEBUG("[MpcTracker]: vertical jerk z update violates constraints: %.2f -> %.2f = %.2f, < %.2f", mpc_x(11), new_mpc_x(11),
+    2564             :                   ((new_mpc_x(11) - mpc_x(11)) / dt1), -constraints.vertical_descending_snap);
+    2565           0 :         problem = true;
+    2566             :       }
+    2567             : 
+    2568       59539 :       if (problem) {
+    2569           0 :         debugPrintState(0.001);
+    2570           0 :         debugPrintMPCResult(0.001);
+    2571             :       }
+    2572             :     }
+    2573             : 
+    2574             :     {
+    2575       59539 :       std::scoped_lock lock(mutex_mpc_x_);
+    2576             : 
+    2577       59539 :       mpc_x_         = new_mpc_x;
+    2578       59539 :       mpc_x_heading_ = new_mpc_x_heading;
+    2579             : 
+    2580       59539 :       mpc_x_heading_(0) = sradians::wrap(mpc_x_heading_(0));
+    2581             :     }
+    2582             :   }
+    2583       59539 : }
+    2584             : 
+    2585             : //}
+    2586             : 
+    2587             : // | -------------------- referece setting -------------------- |
+    2588             : 
+    2589             : /* //{ loadTrajectory() */
+    2590             : 
+    2591             : // method for setting desired trajectory
+    2592         501 : std::tuple<bool, std::string, bool> MpcTracker::loadTrajectory(const mrs_msgs::TrajectoryReference msg) {
+    2593             : 
+    2594         501 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    2595             : 
+    2596             :   // copy the member variables
+    2597        1002 :   auto x         = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    2598        1002 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    2599             : 
+    2600        1002 :   std::stringstream ss;
+    2601             : 
+    2602             :   /* check the trajectory dt //{ */
+    2603             : 
+    2604             :   double trajectory_dt;
+    2605         501 :   if (msg.dt <= 1e-4) {
+    2606           0 :     trajectory_dt = 0.2;
+    2607           0 :     ROS_WARN_THROTTLE(10.0, "[MpcTracker]: the trajectory dt was not specified, assuming its the old 0.2 s");
+    2608         501 :   } else if (msg.dt < dt1) {
+    2609           0 :     trajectory_dt = 0.2;
+    2610           0 :     ss << std::setprecision(3) << "the trajectory dt (" << msg.dt << " s) is too small (smaller than the tracker's internal step size: " << dt1 << " s)";
+    2611           0 :     ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    2612           0 :     return std::tuple(false, ss.str(), false);
+    2613             :   } else {
+    2614         501 :     trajectory_dt = msg.dt;
+    2615             :   }
+    2616             : 
+    2617             :   //}
+    2618             : 
+    2619         501 :   int trajectory_size = msg.points.size();
+    2620             : 
+    2621             :   /* sanitize the time-ness of the trajectory //{ */
+    2622             : 
+    2623         501 :   int    trajectory_sample_offset    = 0;  // how many samples in past is the trajectory
+    2624         501 :   int    trajectory_subsample_offset = 0;  // how many simulation inner loops ahead of the first valid sample
+    2625         501 :   double trajectory_time_offset      = 0;  // how much time in past in [s]
+    2626             : 
+    2627             :   // btw, "trajectory_time_offset = trajectory_dt*trajectory_sample_offset + _dt1_*trajectory_subsample_offset" should hold
+    2628         501 :   if (msg.fly_now) {
+    2629             : 
+    2630         500 :     ros::Time trajectory_time = msg.header.stamp;
+    2631             : 
+    2632             :     // the desired time is 0 => the current time
+    2633             :     // the trajecoty is a single point => the current time
+    2634         500 :     if (trajectory_time == ros::Time(0) || int(msg.points.size()) == 1) {
+    2635             : 
+    2636           2 :       trajectory_time_offset = 0.0;
+    2637             : 
+    2638             :       // the desired time is specified
+    2639             :     } else {
+    2640             : 
+    2641         498 :       trajectory_time_offset = (ros::Time::now() - trajectory_time).toSec();
+    2642             : 
+    2643             :       // when the time offset is negative, thus in the future
+    2644             :       // just say it, but use it like its from the current time
+    2645         498 :       if (trajectory_time_offset < 0.0) {
+    2646             : 
+    2647           0 :         ROS_WARN_THROTTLE(1.0, "[MpcTracker]: received trajectory with timestamp in the future by %.3f s", -trajectory_time_offset);
+    2648             : 
+    2649           0 :         trajectory_time_offset = 0.0;
+    2650             :       }
+    2651             :     }
+    2652             : 
+    2653             :     // if the time offset is set, check if we need to "move the first idx"
+    2654         500 :     if (trajectory_time_offset > 0.0) {
+    2655             : 
+    2656             :       // calculate the offset in samples
+    2657           1 :       trajectory_sample_offset = int(floor(trajectory_time_offset / trajectory_dt));
+    2658             : 
+    2659             :       // and get the subsample offset, which will be used to initialize the interpolator
+    2660           1 :       trajectory_subsample_offset = int(floor(fmod(trajectory_time_offset, trajectory_dt) / dt1));
+    2661             : 
+    2662           1 :       ROS_DEBUG_THROTTLE(0.1, "[MpcTracker]: received trajectory with timestamp in the past by %.3f s",
+    2663             :                          trajectory_dt * trajectory_sample_offset + dt1 * trajectory_subsample_offset);
+    2664             : 
+    2665             :       // if the offset is larger than the number of points in the trajectory
+    2666             :       // the trajectory can not be used
+    2667           1 :       if (trajectory_sample_offset >= trajectory_size) {
+    2668             : 
+    2669           0 :         ss << "trajectory timestamp is too old (time difference = " << trajectory_time_offset << ")";
+    2670           0 :         ROS_ERROR_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    2671           0 :         return std::tuple(false, ss.str(), false);
+    2672             : 
+    2673             :       } else {
+    2674             : 
+    2675             :         // if the offset is larger than one trajectory sample,
+    2676             :         // offset the start
+    2677           1 :         if (trajectory_time_offset >= trajectory_dt) {
+    2678             : 
+    2679             :           // decrease the trajectory size
+    2680           0 :           trajectory_size -= trajectory_sample_offset;
+    2681             : 
+    2682           0 :           ROS_DEBUG_THROTTLE(0.1, "[MpcTracker]: offsetting trajectory by %d samples", trajectory_sample_offset);
+    2683             : 
+    2684             :         } else {
+    2685             : 
+    2686           1 :           trajectory_sample_offset = 0;
+    2687             :         }
+    2688             :       }
+    2689             :     }
+    2690             : 
+    2691             :   } else { // not fly_now
+    2692             : 
+    2693           1 :       trajectory_tracking_in_progress_ = false;
+    2694             :   }
+    2695             : 
+    2696             :   //}
+    2697             : 
+    2698         501 :   ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: trajectory sample offset: %d", trajectory_sample_offset);
+    2699         501 :   ROS_DEBUG_THROTTLE(1.0, "[MpcTracker]: trajectory subsample offset: %d", trajectory_subsample_offset);
+    2700             : 
+    2701             :   // after this, we should have the correct value of
+    2702             :   // * trajectory_size
+    2703             :   // * trajectory_sample_offset
+    2704             :   // * trajectory_subsample_offset
+    2705             : 
+    2706             :   /* copy the trajectory to a local variable //{ */
+    2707             : 
+    2708             :   // copy only the part from the first valid index
+    2709             : 
+    2710        1002 :   MatrixXd des_x_whole_trajectory       = VectorXd::Zero(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2711        1002 :   MatrixXd des_y_whole_trajectory       = VectorXd::Zero(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2712        1002 :   MatrixXd des_z_whole_trajectory       = VectorXd::Zero(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2713        1002 :   MatrixXd des_heading_whole_trajectory = VectorXd::Zero(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2714             : 
+    2715       25931 :   for (int i = 0; i < trajectory_size; i++) {
+    2716             : 
+    2717       25430 :     des_x_whole_trajectory(i)       = msg.points[trajectory_sample_offset + i].position.x;
+    2718       25430 :     des_y_whole_trajectory(i)       = msg.points[trajectory_sample_offset + i].position.y;
+    2719       25430 :     des_z_whole_trajectory(i)       = msg.points[trajectory_sample_offset + i].position.z;
+    2720       25430 :     des_heading_whole_trajectory(i) = msg.points[trajectory_sample_offset + i].heading;
+    2721             :   }
+    2722             : 
+    2723             :   //}
+    2724             : 
+    2725             :   /* set looping //{ */
+    2726             : 
+    2727         501 :   bool loop = false;
+    2728             : 
+    2729         501 :   if (msg.loop) {
+    2730             : 
+    2731           0 :     double first_x = des_x_whole_trajectory(0);
+    2732           0 :     double first_y = des_y_whole_trajectory(0);
+    2733           0 :     double first_z = des_z_whole_trajectory(0);
+    2734           0 :     double first_hdg = des_heading_whole_trajectory(0);
+    2735             : 
+    2736           0 :     double last_x = des_x_whole_trajectory(trajectory_size - 1);
+    2737           0 :     double last_y = des_y_whole_trajectory(trajectory_size - 1);
+    2738           0 :     double last_z = des_z_whole_trajectory(trajectory_size - 1);
+    2739           0 :     double last_hdg = des_heading_whole_trajectory(trajectory_size - 1);
+    2740             : 
+    2741             :     // check whether the trajectory is loopable
+    2742           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) {
+    2743             : 
+    2744           0 :       ROS_INFO_THROTTLE(1.0, "[MpcTracker]: looping enabled");
+    2745           0 :       loop = true;
+    2746             : 
+    2747             :     } else {
+    2748             : 
+    2749           0 :       ss << "can not loop trajectory, the first and last points are too far apart";
+    2750           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    2751           0 :       return std::tuple(false, ss.str(), false);
+    2752             :     }
+    2753             : 
+    2754             :   } else {
+    2755             : 
+    2756         501 :     loop = false;
+    2757             :   }
+    2758             : 
+    2759             :   //}
+    2760             : 
+    2761             :   // by this time, the values of these should be set:
+    2762             :   // * loop
+    2763             : 
+    2764             :   /* add tail (the last point repeated to fill the prediction horizon) //{ */
+    2765             : 
+    2766         501 :   if (!loop) {
+    2767             : 
+    2768             :     // extend it so it has smooth ending
+    2769       20541 :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2770             : 
+    2771       20040 :       des_x_whole_trajectory(i + trajectory_size)       = des_x_whole_trajectory(i + trajectory_size - 1);
+    2772       20040 :       des_y_whole_trajectory(i + trajectory_size)       = des_y_whole_trajectory(i + trajectory_size - 1);
+    2773       20040 :       des_z_whole_trajectory(i + trajectory_size)       = des_z_whole_trajectory(i + trajectory_size - 1);
+    2774       20040 :       des_heading_whole_trajectory(i + trajectory_size) = des_heading_whole_trajectory(i + trajectory_size - 1);
+    2775             :     }
+    2776             :   }
+    2777             : 
+    2778             :   //}
+    2779             : 
+    2780             :   // by this time, the values of these should be set correctly:
+    2781             :   // * trajectory_size
+    2782             :   // * des_x_whole_trajectory
+    2783             :   // * des_y_whole_trajectory
+    2784             :   // * des_z_whole_trajectory
+    2785             :   // * des_heading_whole_trajectory
+    2786             : 
+    2787             :   /* update the global variables //{ */
+    2788             : 
+    2789             :   {
+    2790        1002 :     std::scoped_lock lock(mutex_des_whole_trajectory_, mutex_des_trajectory_, mutex_trajectory_tracking_states_);
+    2791             : 
+    2792         501 :     des_whole_trajectory_id_ = msg.input_id;
+    2793             : 
+    2794         501 :     auto mpc_x_heading = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_heading_);
+    2795             : 
+    2796         501 :     trajectory_tracking_in_progress_ = msg.fly_now;
+    2797         501 :     trajectory_track_heading_        = msg.use_heading;
+    2798             : 
+    2799             :     // allocate the vectors
+    2800         501 :     des_x_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2801         501 :     des_y_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2802         501 :     des_z_whole_trajectory_       = std::make_shared<VectorXd>(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2803         501 :     des_heading_whole_trajectory_ = std::make_shared<VectorXd>(trajectory_size + MPC_HORIZON_LENGTH, 1);
+    2804             : 
+    2805       45971 :     for (int i = 0; i < trajectory_size + MPC_HORIZON_LENGTH; i++) {
+    2806             : 
+    2807       45470 :       (*des_x_whole_trajectory_)(i) = des_x_whole_trajectory(i);
+    2808       45470 :       (*des_y_whole_trajectory_)(i) = des_y_whole_trajectory(i);
+    2809       45470 :       (*des_z_whole_trajectory_)(i) = des_z_whole_trajectory(i);
+    2810             : 
+    2811       45470 :       if (trajectory_track_heading_) {
+    2812       45470 :         (*des_heading_whole_trajectory_)(i) = des_heading_whole_trajectory(i);
+    2813             :       } else {
+    2814           0 :         (*des_heading_whole_trajectory_).fill(mpc_x_heading(0, 0));
+    2815             :       }
+    2816             :     }
+    2817             : 
+    2818             :     // if we are tracking trajectory, copy the setpoint
+    2819         501 :     if (trajectory_tracking_in_progress_) {
+    2820             : 
+    2821         500 :       toggleHover(false);
+    2822             : 
+    2823             :       /* interpolate the trajectory points and fill in the desired_trajectory vector //{ */
+    2824             : 
+    2825       20500 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    2826             : 
+    2827       20000 :         double first_time = dt1 + i * _dt2_ + trajectory_subsample_offset * dt1;
+    2828             : 
+    2829       20000 :         int first_idx  = floor(first_time / trajectory_dt);
+    2830       20000 :         int second_idx = first_idx + 1;
+    2831             : 
+    2832       20000 :         double interp_coeff = std::fmod(first_time / trajectory_dt, 1.0);
+    2833             : 
+    2834       20000 :         if (trajectory_tracking_loop_) {
+    2835             : 
+    2836           0 :           if (second_idx >= trajectory_size) {
+    2837           0 :             second_idx -= trajectory_size;
+    2838             :           }
+    2839             : 
+    2840           0 :           if (first_idx >= trajectory_size) {
+    2841           0 :             first_idx -= trajectory_size;
+    2842             :           }
+    2843             :         } else {
+    2844             : 
+    2845       20000 :           if (second_idx >= trajectory_size) {
+    2846           0 :             second_idx = trajectory_size - 1;
+    2847             :           }
+    2848             : 
+    2849       20000 :           if (first_idx >= trajectory_size) {
+    2850           0 :             first_idx = trajectory_size - 1;
+    2851             :           }
+    2852             :         }
+    2853             : 
+    2854       20000 :         des_x_trajectory_(i, 0) = (1 - interp_coeff) * des_x_whole_trajectory(first_idx) + interp_coeff * des_x_whole_trajectory(second_idx);
+    2855       20000 :         des_y_trajectory_(i, 0) = (1 - interp_coeff) * des_y_whole_trajectory(first_idx) + interp_coeff * des_y_whole_trajectory(second_idx);
+    2856       20000 :         des_z_trajectory_(i, 0) = (1 - interp_coeff) * des_z_whole_trajectory(first_idx) + interp_coeff * des_z_whole_trajectory(second_idx);
+    2857             : 
+    2858       20000 :         des_heading_trajectory_(i, 0) = sradians::interp(des_heading_whole_trajectory(first_idx), des_heading_whole_trajectory(second_idx), interp_coeff);
+    2859             :       }
+    2860             : 
+    2861             :       //}
+    2862             :     }
+    2863             : 
+    2864         501 :     trajectory_size_             = trajectory_size;
+    2865         501 :     trajectory_current_time_     = 0;
+    2866         501 :     trajectory_set_              = true;
+    2867         501 :     trajectory_tracking_loop_    = loop;
+    2868         501 :     trajectory_dt_               = trajectory_dt;
+    2869         501 :     trajectory_count_++;
+    2870             :   }
+    2871             : 
+    2872             :   //}
+    2873             : 
+    2874         501 :   ROS_INFO_THROTTLE(1, "[MpcTracker]: finished setting trajectory with length %d", trajectory_size);
+    2875             : 
+    2876             :   /* publish the debugging topics of the post-processed trajectory //{ */
+    2877             : 
+    2878             :   {
+    2879             : 
+    2880        1002 :     geometry_msgs::PoseArray debug_trajectory_out;
+    2881         501 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    2882         501 :     debug_trajectory_out.header.frame_id = common_handlers_->transformer->resolveFrame(msg.header.frame_id);
+    2883             : 
+    2884             :     {
+    2885        1002 :       std::scoped_lock lock(mutex_des_whole_trajectory_);
+    2886             : 
+    2887       25931 :       for (int i = 0; i < trajectory_size; i++) {
+    2888             : 
+    2889       25430 :         geometry_msgs::Pose new_pose;
+    2890             : 
+    2891       25430 :         new_pose.position.x = (*des_x_whole_trajectory_)(i);
+    2892       25430 :         new_pose.position.y = (*des_y_whole_trajectory_)(i);
+    2893       25430 :         new_pose.position.z = (*des_z_whole_trajectory_)(i);
+    2894             : 
+    2895       25430 :         new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, (*des_heading_whole_trajectory_)(i));
+    2896             : 
+    2897       25430 :         debug_trajectory_out.poses.push_back(new_pose);
+    2898             :       }
+    2899             :     }
+    2900             : 
+    2901         501 :     pub_debug_processed_trajectory_poses_.publish(debug_trajectory_out);
+    2902             : 
+    2903        1002 :     visualization_msgs::MarkerArray msg_out;
+    2904             : 
+    2905        1002 :     visualization_msgs::Marker marker;
+    2906             : 
+    2907         501 :     marker.header.stamp     = ros::Time::now();
+    2908         501 :     marker.header.frame_id  = common_handlers_->transformer->resolveFrame(msg.header.frame_id);
+    2909         501 :     marker.type             = visualization_msgs::Marker::LINE_LIST;
+    2910         501 :     marker.color.a          = 1;
+    2911         501 :     marker.scale.x          = 0.05;
+    2912         501 :     marker.color.r          = 1;
+    2913         501 :     marker.color.g          = 0;
+    2914         501 :     marker.color.b          = 0;
+    2915         501 :     marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    2916             : 
+    2917             :     {
+    2918        1002 :       std::scoped_lock lock(mutex_des_whole_trajectory_);
+    2919             : 
+    2920       25430 :       for (int i = 0; i < trajectory_size - 1; i++) {
+    2921             : 
+    2922       24929 :         geometry_msgs::Point point1;
+    2923             : 
+    2924       24929 :         point1.x = des_x_whole_trajectory(i);
+    2925       24929 :         point1.y = des_y_whole_trajectory(i);
+    2926       24929 :         point1.z = des_z_whole_trajectory(i);
+    2927             : 
+    2928       24929 :         marker.points.push_back(point1);
+    2929             : 
+    2930       24929 :         geometry_msgs::Point point2;
+    2931             : 
+    2932       24929 :         point2.x = des_x_whole_trajectory(i + 1);
+    2933       24929 :         point2.y = des_y_whole_trajectory(i + 1);
+    2934       24929 :         point2.z = des_z_whole_trajectory(i + 1);
+    2935             : 
+    2936       24929 :         marker.points.push_back(point2);
+    2937             :       }
+    2938             :     }
+    2939             : 
+    2940         501 :     msg_out.markers.push_back(marker);
+    2941             : 
+    2942         501 :     pub_debug_processed_trajectory_markers_.publish(msg_out);
+    2943             :   }
+    2944             : 
+    2945             :   //}
+    2946             : 
+    2947         501 :   publishDiagnostics();
+    2948             : 
+    2949        1002 :   return std::tuple(true, "trajectory loaded", false);
+    2950             : }
+    2951             : 
+    2952             : //}
+    2953             : 
+    2954             : /* //{ setSinglePointReference() */
+    2955             : 
+    2956             : // fill the des_*_trajectory based on a single point
+    2957         402 : void MpcTracker::setSinglePointReference(const double x, const double y, const double z, const double heading) {
+    2958             : 
+    2959         804 :   std::scoped_lock lock(mutex_des_trajectory_);
+    2960             : 
+    2961         402 :   des_x_trajectory_.fill(x);
+    2962         402 :   des_y_trajectory_.fill(y);
+    2963         402 :   des_z_trajectory_.fill(z);
+    2964         402 :   des_heading_trajectory_.fill(heading);
+    2965         402 : }
+    2966             : 
+    2967             : //}
+    2968             : 
+    2969             : /* //{ setGoal() */
+    2970             : 
+    2971             : // set absolute goal
+    2972         245 : void MpcTracker::setGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading) {
+    2973             : 
+    2974         245 :   double desired_heading = sradians::wrap(heading);
+    2975             : 
+    2976         490 :   auto mpc_x_heading = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_heading_);
+    2977             : 
+    2978         245 :   if (!use_heading) {
+    2979           0 :     desired_heading = mpc_x_heading(0, 0);
+    2980             :   }
+    2981             : 
+    2982         245 :   trajectory_tracking_in_progress_ = false;
+    2983             : 
+    2984         245 :   setSinglePointReference(pos_x, pos_y, pos_z, desired_heading);
+    2985             : 
+    2986         245 :   publishDiagnostics();
+    2987         245 : }
+    2988             : 
+    2989             : //}
+    2990             : 
+    2991             : /* //{ setRelativeGoal() */
+    2992             : 
+    2993         157 : void MpcTracker::setRelativeGoal(const double pos_x, const double pos_y, const double pos_z, const double heading, const bool use_heading) {
+    2994             : 
+    2995         314 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    2996             : 
+    2997         157 :   double abs_x = mpc_x(0, 0) + pos_x;
+    2998         157 :   double abs_y = mpc_x(4, 0) + pos_y;
+    2999         157 :   double abs_z = mpc_x(8, 0) + pos_z;
+    3000             : 
+    3001         157 :   double abs_heading = mpc_x_heading(0, 0);
+    3002             : 
+    3003         157 :   if (use_heading) {
+    3004           0 :     abs_heading += heading;
+    3005             :   }
+    3006             : 
+    3007         157 :   trajectory_tracking_in_progress_ = false;
+    3008             : 
+    3009         157 :   setSinglePointReference(abs_x, abs_y, abs_z, abs_heading);
+    3010             : 
+    3011         157 :   publishDiagnostics();
+    3012         157 : }
+    3013             : 
+    3014             : //}
+    3015             : 
+    3016             : /* toggleHover() //{ */
+    3017             : 
+    3018         906 : void MpcTracker::toggleHover(bool in) {
+    3019             : 
+    3020         906 :   if (in == false && hovering_in_progress_) {
+    3021             : 
+    3022          77 :     ROS_DEBUG("[MpcTracker]: stoppping the hover timer");
+    3023             : 
+    3024          77 :     timer_hover_.stop();
+    3025             : 
+    3026          77 :     hovering_in_progress_ = false;
+    3027             : 
+    3028         829 :   } else if (in == true && !hovering_in_progress_) {
+    3029             : 
+    3030          77 :     ROS_DEBUG("[MpcTracker]: starting the hover timer");
+    3031             : 
+    3032          77 :     hovering_in_progress_ = true;
+    3033             : 
+    3034          77 :     timer_hover_.start();
+    3035             :   }
+    3036         906 : }
+    3037             : 
+    3038             : //}
+    3039             : 
+    3040             : // | ------------------- trajectory tracking ------------------ |
+    3041             : 
+    3042             : /* startTrajectoryTrackingImpl() //{ */
+    3043             : 
+    3044           1 : std::tuple<bool, std::string> MpcTracker::startTrajectoryTrackingImpl(void) {
+    3045             : 
+    3046           2 :   std::stringstream ss;
+    3047             : 
+    3048           1 :   if (trajectory_set_) {
+    3049             : 
+    3050           1 :     toggleHover(false);
+    3051             : 
+    3052             :     {
+    3053           1 :       std::scoped_lock lock(mutex_des_trajectory_);
+    3054             : 
+    3055           1 :       trajectory_tracking_in_progress_ = true;
+    3056           1 :       trajectory_current_time_ = 0;
+    3057             :     }
+    3058             : 
+    3059           1 :     publishDiagnostics();
+    3060             : 
+    3061           1 :     ss << "trajectory tracking started";
+    3062           1 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3063             : 
+    3064           1 :     return std::tuple(true, ss.str());
+    3065             : 
+    3066             :   } else {
+    3067             : 
+    3068           0 :     ss << "can not start trajectory tracking, the trajectory is not set";
+    3069           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3070             : 
+    3071           0 :     return std::tuple(false, ss.str());
+    3072             :   }
+    3073             : }
+    3074             : 
+    3075             : //}
+    3076             : 
+    3077             : /* resumeTrajectoryTrackingImpl() //{ */
+    3078             : 
+    3079           0 : std::tuple<bool, std::string> MpcTracker::resumeTrajectoryTrackingImpl(void) {
+    3080             : 
+    3081           0 :   std::stringstream ss;
+    3082             : 
+    3083           0 :   if (trajectory_set_) {
+    3084             : 
+    3085           0 :     toggleHover(false);
+    3086             : 
+    3087           0 :     int trajectory_tracking_idx = getCurrentTrajectoryIdx();
+    3088             : 
+    3089           0 :     if (trajectory_tracking_idx < (trajectory_size_ - 1)) {
+    3090             : 
+    3091             :       {
+    3092           0 :         std::scoped_lock lock(mutex_des_trajectory_);
+    3093             : 
+    3094           0 :         trajectory_tracking_in_progress_ = true;
+    3095             :       }
+    3096             : 
+    3097           0 :       ss << "trajectory tracking resumed";
+    3098           0 :       ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3099             : 
+    3100           0 :       publishDiagnostics();
+    3101             : 
+    3102           0 :       return std::tuple(true, ss.str());
+    3103             : 
+    3104             :     } else {
+    3105             : 
+    3106           0 :       ss << "can not resume trajectory tracking, trajectory is already finished";
+    3107           0 :       ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3108             : 
+    3109           0 :       return std::tuple(false, ss.str());
+    3110             :     }
+    3111             : 
+    3112             :   } else {
+    3113             : 
+    3114           0 :     ss << "can not resume trajectory tracking, ther trajectory is not set";
+    3115           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3116             : 
+    3117           0 :     return std::tuple(false, ss.str());
+    3118             :   }
+    3119             : }
+    3120             : 
+    3121             : //}
+    3122             : 
+    3123             : /* stopTrajectoryTrackingImpl() //{ */
+    3124             : 
+    3125           0 : std::tuple<bool, std::string> MpcTracker::stopTrajectoryTrackingImpl(void) {
+    3126             : 
+    3127           0 :   std::stringstream ss;
+    3128             : 
+    3129           0 :   if (trajectory_tracking_in_progress_) {
+    3130             : 
+    3131           0 :     trajectory_tracking_in_progress_ = false;
+    3132             : 
+    3133           0 :     toggleHover(true);
+    3134             : 
+    3135           0 :     ss << "stopping trajectory tracking";
+    3136           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3137             : 
+    3138           0 :     publishDiagnostics();
+    3139             : 
+    3140             :   } else {
+    3141             : 
+    3142           0 :     ss << "can not stop trajectory tracking, already at stop";
+    3143           0 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3144             :   }
+    3145             : 
+    3146           0 :   return std::tuple(true, ss.str());
+    3147             : }
+    3148             : 
+    3149             : //}
+    3150             : 
+    3151             : /* gotoTrajectoryStartImpl() //{ */
+    3152             : 
+    3153           1 : std::tuple<bool, std::string> MpcTracker::gotoTrajectoryStartImpl(void) {
+    3154             : 
+    3155           2 :   std::stringstream ss;
+    3156             : 
+    3157           1 :   if (trajectory_set_) {
+    3158             : 
+    3159           1 :     toggleHover(false);
+    3160             : 
+    3161           1 :     trajectory_tracking_in_progress_ = false;
+    3162             : 
+    3163             :     {
+    3164           2 :       std::scoped_lock lock(mutex_des_whole_trajectory_);
+    3165             : 
+    3166           1 :       setGoal((*des_x_whole_trajectory_)[0], (*des_y_whole_trajectory_)[0], (*des_z_whole_trajectory_)[0], (*des_heading_whole_trajectory_)[0],
+    3167           1 :               trajectory_track_heading_);
+    3168             :     }
+    3169             : 
+    3170           1 :     publishDiagnostics();
+    3171             : 
+    3172           1 :     ss << "flying to the start of the trajectory";
+    3173           1 :     ROS_INFO_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3174             : 
+    3175           1 :     return std::tuple(true, ss.str());
+    3176             : 
+    3177             :   } else {
+    3178             : 
+    3179           0 :     ss << "can not fly to the start of the trajectory, the trajectory is not set";
+    3180           0 :     ROS_WARN_STREAM_THROTTLE(1.0, "[MpcTracker]: " << ss.str());
+    3181             : 
+    3182           0 :     return std::tuple(false, ss.str());
+    3183             :   }
+    3184             : }
+    3185             : 
+    3186             : //}
+    3187             : 
+    3188             : // | ------------------------- support ------------------------ |
+    3189             : 
+    3190             : /* //{ publishDiagnostics() */
+    3191             : 
+    3192       19137 : void MpcTracker::publishDiagnostics(void) {
+    3193             : 
+    3194       38274 :   auto des_x_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_x_trajectory_);
+    3195       38274 :   auto des_y_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_y_trajectory_);
+    3196       38274 :   auto des_z_trajectory       = mrs_lib::get_mutexed(mutex_des_trajectory_, des_z_trajectory_);
+    3197       38274 :   auto des_heading_trajectory = mrs_lib::get_mutexed(mutex_des_trajectory_, des_heading_trajectory_);
+    3198             : 
+    3199       38274 :   mrs_msgs::MpcTrackerDiagnostics diagnostics;
+    3200             : 
+    3201       19137 :   diagnostics.header.stamp    = ros::Time::now();
+    3202       19137 :   diagnostics.header.frame_id = uav_state_.header.frame_id;
+    3203             : 
+    3204       19137 :   diagnostics.active = is_active_;
+    3205             : 
+    3206       19137 :   diagnostics.uav_name = _uav_name_;
+    3207             : 
+    3208       19137 :   diagnostics.collision_avoidance_active = collision_avoidance_enabled_;
+    3209       19137 :   diagnostics.avoiding_collision         = collision_avoidance_affecting_me_;
+    3210             : 
+    3211       19137 :   diagnostics.setpoint.position.x = des_x_trajectory(0, 0);
+    3212       19137 :   diagnostics.setpoint.position.y = des_y_trajectory(0, 0);
+    3213       19137 :   diagnostics.setpoint.position.z = des_z_trajectory(0, 0);
+    3214             : 
+    3215       19137 :   diagnostics.setpoint.orientation = mrs_lib::AttitudeConverter(0, 0, des_heading_trajectory(0, 0));
+    3216             : 
+    3217       38274 :   std::stringstream ss;
+    3218             : 
+    3219             :   {
+    3220       38274 :     std::scoped_lock lock(mutex_other_uav_diagnostics_);
+    3221             : 
+    3222             :     // fill in if other UAVs are sending their trajectories
+    3223       19137 :     std::map<std::string, mrs_msgs::MpcTrackerDiagnostics>::iterator u = other_uav_diagnostics_.begin();
+    3224             : 
+    3225       21102 :     while (u != other_uav_diagnostics_.end()) {
+    3226             : 
+    3227        1965 :       if (u->second.collision_avoidance_active) {
+    3228             : 
+    3229             :         // is the other's trajectory fresh enought?
+    3230        1255 :         if ((ros::Time::now() - u->second.header.stamp).toSec() < _collision_trajectory_timeout_) {
+    3231        1255 :           diagnostics.avoidance_active_uavs.push_back(u->first);
+    3232        1255 :           ss << u->first.c_str() << ", ";
+    3233             :         }
+    3234             :       }
+    3235             : 
+    3236        1965 :       u++;
+    3237             :     }
+    3238             :   }
+    3239             : 
+    3240       38274 :   auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3241             : 
+    3242       19137 :   if (ss.str().length() > 0) {
+    3243        1255 :     ROS_DEBUG_STREAM_THROTTLE(5.0, "[MpcTracker]: getting avoidance trajectories: " << ss.str());
+    3244       38657 :   } else if (collision_avoidance_enabled_ &&
+    3245       20775 :       (uav_state.estimator_horizontal == "lat_gps" || uav_state.estimator_horizontal == "lat_rtk")) {
+    3246       14846 :     ROS_DEBUG_THROTTLE(10.0, "[MpcTracker]: missing avoidance trajectories!");
+    3247             :   }
+    3248             : 
+    3249       19137 :   pub_diagnostics_.publish(diagnostics);
+    3250             : 
+    3251       38274 :   std_msgs::String string_msg;
+    3252             : 
+    3253       19137 :   if (diagnostics.avoidance_active_uavs.empty()) {
+    3254             : 
+    3255       17882 :     string_msg.data = "-id col_avoid I see: NOTHING";
+    3256             : 
+    3257             :   } else {
+    3258             : 
+    3259        1255 :     string_msg.data = "-id col_avoid I see: ";
+    3260             :   }
+    3261             : 
+    3262       19137 :   if (diagnostics.avoidance_active_uavs.size() <= 3) {
+    3263             : 
+    3264       20392 :     for (size_t i = 0; i < diagnostics.avoidance_active_uavs.size(); i++) {
+    3265        1255 :       if (i == 0) {
+    3266        1255 :         string_msg.data += diagnostics.avoidance_active_uavs[i];
+    3267             :       } else {
+    3268           0 :         string_msg.data += ", " + diagnostics.avoidance_active_uavs[i];
+    3269             :       }
+    3270             :     }
+    3271             : 
+    3272             :   } else {
+    3273             : 
+    3274           0 :     std::stringstream ss;
+    3275           0 :     ss << diagnostics.avoidance_active_uavs.size();
+    3276             : 
+    3277           0 :     string_msg.data += ss.str() + " UAVs";
+    3278             :   }
+    3279             : 
+    3280       19137 :   pub_status_string_.publish(string_msg);
+    3281       19137 : }
+    3282             : 
+    3283             : //}
+    3284             : 
+    3285             : /* debugPrintState() //{ */
+    3286             : 
+    3287           0 : void MpcTracker::debugPrintState(const double throttle) {
+    3288             : 
+    3289           0 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    3290             : 
+    3291           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));
+    3292           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));
+    3293           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));
+    3294           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));
+    3295           0 : }
+    3296             : 
+    3297             : //}
+    3298             : 
+    3299             : /* debugPrintMPCu() //{ */
+    3300             : 
+    3301           0 : void MpcTracker::debugPrintMPCResult(const double throttle) {
+    3302             : 
+    3303           0 :   auto [mpc_u, mpc_u_heading] = mrs_lib::get_mutexed(mutex_mpc_u_, mpc_u_, mpc_u_heading_);
+    3304           0 :   auto constraints            = mrs_lib::get_mutexed(mutex_constraints_, constraints_);
+    3305             : 
+    3306           0 :   ROS_DEBUG_THROTTLE(throttle, "[MpcTracker]: MPC result: [%.2f, %.2f, %.2f, %.2f]", mpc_u(0), mpc_u(1), mpc_u(2), mpc_u_heading);
+    3307           0 :   ROS_DEBUG_THROTTLE(throttle, "[MpcTracker]: snap constraint: hor: %.2f, ver asc: %.2f, vert desc: %.2f, heading: %.2f]", constraints.horizontal_snap,
+    3308             :                      constraints.vertical_ascending_snap, constraints.vertical_descending_snap, constraints.heading_snap);
+    3309           0 : }
+    3310             : 
+    3311             : //}
+    3312             : 
+    3313             : /* getCurrentTrajectoryIdx() //{ */
+    3314             : 
+    3315        6957 : int MpcTracker::getCurrentTrajectoryIdx() {
+    3316             : 
+    3317        6957 :   auto trajectory_current_time = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_current_time_);
+    3318        6957 :   auto trajectory_dt   = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_dt_);
+    3319             : 
+    3320        6957 :   return floor(trajectory_current_time / trajectory_dt);
+    3321             : }
+    3322             : 
+    3323             : //}
+    3324             : 
+    3325             : /* increaseCurrentTrajectoryTime() //{ */
+    3326             : 
+    3327       12195 : void MpcTracker::increaseCurrentTrajectoryTime(const double dt) {
+    3328             : 
+    3329       12195 :   auto trajectory_current_time = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_current_time_);
+    3330       12195 :   auto [trajectory_size, trajectory_dt] = mrs_lib::get_mutexed(mutex_des_trajectory_, trajectory_size_, trajectory_dt_);
+    3331             : 
+    3332       12195 :   trajectory_current_time += dt;
+    3333             : 
+    3334       12195 :   const double trajectory_duration = trajectory_size * trajectory_dt;
+    3335             : 
+    3336             :   // if the tracking idx hits the end of the trajectory
+    3337       12195 :   if (trajectory_current_time >= trajectory_duration) {
+    3338             : 
+    3339           4 :     if (trajectory_tracking_loop_) {
+    3340             : 
+    3341             :       // reset the idx
+    3342           0 :       trajectory_current_time -= trajectory_duration;
+    3343             : 
+    3344           0 :       ROS_INFO("[MpcTracker]: trajectory looped");
+    3345             : 
+    3346             :     } else {
+    3347             : 
+    3348           4 :       trajectory_tracking_in_progress_ = false;
+    3349             : 
+    3350           4 :       ROS_INFO("[MpcTracker]: done tracking trajectory");
+    3351             :     }
+    3352             :   }
+    3353             : 
+    3354       12195 :   mrs_lib::set_mutexed(mutex_trajectory_tracking_states_, trajectory_current_time, trajectory_current_time_);
+    3355       12195 : }
+    3356             : 
+    3357             : //}
+    3358             : 
+    3359             : // --------------------------------------------------------------
+    3360             : // |                           timers                           |
+    3361             : // --------------------------------------------------------------
+    3362             : 
+    3363             : /* //{ timerDiagnostics() */
+    3364             : 
+    3365             : // published diagnostics in reguar intervals
+    3366       18196 : void MpcTracker::timerDiagnostics(const ros::TimerEvent& event) {
+    3367             : 
+    3368       18196 :   if (!is_initialized_)
+    3369           0 :     return;
+    3370             : 
+    3371       54588 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerDiagnostics", _diagnostics_rate_, 0.1, event);
+    3372       54588 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerDiagnostics", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3373             : 
+    3374       18196 :   publishDiagnostics();
+    3375             : }
+    3376             : 
+    3377             : //}
+    3378             : 
+    3379             : /* //{ timerMPC() */
+    3380             : 
+    3381       65275 : void MpcTracker::timerMPC(const ros::TimerEvent& event) {
+    3382             : 
+    3383       65275 :   if (odometry_reset_in_progress_) {
+    3384           0 :     ROS_ERROR("[MpcTracker]: mpc iteration tried run while reseting odometry");
+    3385           0 :     return;
+    3386             :   }
+    3387             : 
+    3388       65275 :   auto dt1 = mrs_lib::get_mutexed(mutex_dt1_, dt1_);
+    3389             : 
+    3390       65275 :   mrs_lib::AtomicScopeFlag unset_running(mpc_timer_running_);
+    3391             : 
+    3392       65275 :   bool started_with_invalid = mpc_result_invalid_;
+    3393             : 
+    3394       65275 :   if (!is_active_) {
+    3395           0 :     return;
+    3396             :   }
+    3397             : 
+    3398       65275 :   if (!is_initialized_) {
+    3399           0 :     return;
+    3400             :   }
+    3401             : 
+    3402      195825 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerMPC", 1.0 / dt1, 0.01, event);
+    3403      195825 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerMPC", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3404             : 
+    3405       65275 :   ros::Time     begin = ros::Time::now();
+    3406       65275 :   ros::Time     end;
+    3407       65275 :   ros::Duration interval;
+    3408             :   int           trajectory_id;
+    3409             : 
+    3410             :   // if we are tracking trajectory, copy the setpoint
+    3411       65275 :   if (trajectory_tracking_in_progress_) {
+    3412             : 
+    3413       24390 :     MatrixXd des_x_trajectory, des_y_trajectory, des_z_trajectory, des_heading_trajectory;
+    3414       24390 :     VectorXd des_x_whole_trajectory, des_y_whole_trajectory, des_z_whole_trajectory, des_heading_whole_trajectory;
+    3415             :     double   trajectory_dt;
+    3416             :     int      trajectory_size;
+    3417             :     {
+    3418       12195 :       std::scoped_lock lock(mutex_des_trajectory_, mutex_des_whole_trajectory_);
+    3419             : 
+    3420       12195 :       des_x_trajectory       = des_x_trajectory_;
+    3421       12195 :       des_y_trajectory       = des_y_trajectory_;
+    3422       12195 :       des_z_trajectory       = des_z_trajectory_;
+    3423       12195 :       des_heading_trajectory = des_heading_trajectory_;
+    3424             : 
+    3425       12195 :       des_x_whole_trajectory       = *des_x_whole_trajectory_;
+    3426       12195 :       des_y_whole_trajectory       = *des_y_whole_trajectory_;
+    3427       12195 :       des_z_whole_trajectory       = *des_z_whole_trajectory_;
+    3428       12195 :       des_heading_whole_trajectory = *des_heading_whole_trajectory_;
+    3429             : 
+    3430       12195 :       trajectory_size = trajectory_size_;
+    3431       12195 :       trajectory_dt   = trajectory_dt_;
+    3432             : 
+    3433       12195 :       trajectory_id = des_whole_trajectory_id_;
+    3434             :     }
+    3435             : 
+    3436             :     /* interpolate the trajectory points and fill in the desired_trajectory vector //{ */
+    3437             : 
+    3438       12195 :     const double dt_from_last_update = (event.current_real - event.last_real).toSec();
+    3439             : 
+    3440       12195 :     increaseCurrentTrajectoryTime(dt_from_last_update);
+    3441             : 
+    3442       12195 :     auto trajectory_current_time = mrs_lib::get_mutexed(mutex_trajectory_tracking_states_, trajectory_current_time_);
+    3443             : 
+    3444      499995 :     for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    3445             : 
+    3446      487800 :       double first_time = trajectory_current_time + double(i) * _dt2_;
+    3447             : 
+    3448      487800 :       int first_idx  = int(floor(first_time / trajectory_dt));
+    3449      487800 :       int second_idx = first_idx + 1;
+    3450             : 
+    3451      487800 :       double interp_coeff = std::fmod(first_time / trajectory_dt, 1.0);
+    3452             : 
+    3453      487800 :       if (trajectory_tracking_loop_) {
+    3454             : 
+    3455           0 :         if (second_idx >= trajectory_size) {
+    3456           0 :           second_idx = second_idx % trajectory_size;
+    3457             :         }
+    3458             : 
+    3459           0 :         if (first_idx >= trajectory_size) {
+    3460           0 :           first_idx = first_idx % trajectory_size;
+    3461             :         }
+    3462             : 
+    3463             :       } else {
+    3464             : 
+    3465      487800 :         if (second_idx >= trajectory_size) {
+    3466       61163 :           second_idx = trajectory_size - 1;
+    3467             :         }
+    3468             : 
+    3469      487800 :         if (first_idx >= trajectory_size) {
+    3470       58178 :           first_idx = trajectory_size - 1;
+    3471             :         }
+    3472             :       }
+    3473             : 
+    3474      487800 :       des_x_trajectory(i, 0) = (1 - interp_coeff) * des_x_whole_trajectory[first_idx] + interp_coeff * des_x_whole_trajectory[second_idx];
+    3475      487800 :       des_y_trajectory(i, 0) = (1 - interp_coeff) * des_y_whole_trajectory[first_idx] + interp_coeff * des_y_whole_trajectory[second_idx];
+    3476      487800 :       des_z_trajectory(i, 0) = (1 - interp_coeff) * des_z_whole_trajectory[first_idx] + interp_coeff * des_z_whole_trajectory[second_idx];
+    3477             : 
+    3478      487800 :       des_heading_trajectory(i, 0) = sradians::interp(des_heading_whole_trajectory[first_idx], des_heading_whole_trajectory[second_idx], interp_coeff);
+    3479             :     }
+    3480             : 
+    3481             :     {
+    3482       24390 :       std::scoped_lock lock(mutex_des_trajectory_);
+    3483             : 
+    3484       12195 :       des_x_trajectory_       = des_x_trajectory;
+    3485       12195 :       des_y_trajectory_       = des_y_trajectory;
+    3486       12195 :       des_z_trajectory_       = des_z_trajectory;
+    3487       12195 :       des_heading_trajectory_ = des_heading_trajectory;
+    3488             :     }
+    3489             : 
+    3490             :     //}
+    3491             : 
+    3492             :   } else {
+    3493             : 
+    3494       53080 :     std::scoped_lock lock(mutex_des_whole_trajectory_);
+    3495             : 
+    3496       53080 :     trajectory_id = des_whole_trajectory_id_;
+    3497             :   }
+    3498             : 
+    3499       65275 :   manageConstraints();
+    3500             : 
+    3501       65275 :   calculateMPC();
+    3502             : 
+    3503       65275 :   end      = ros::Time::now();
+    3504       65275 :   interval = end - begin;
+    3505             : 
+    3506             :   // | ------------------ calculate the MPC RTF ----------------- |
+    3507             : 
+    3508       65275 :   mpc_rtf_ = 0.99 * mpc_rtf_ + 0.01 * (interval.toSec()/dt1);
+    3509             : 
+    3510       65275 :   if (mpc_rtf_ >= 1.0) {
+    3511           0 :     ROS_WARN_THROTTLE(5.0, "[MpcTracker] MPC Real Time Factor (%.3f) is slow", mpc_rtf_);
+    3512             :   }
+    3513             : 
+    3514             :   /* publish predicted future //{ */
+    3515             : 
+    3516             :   {
+    3517      130550 :     geometry_msgs::PoseArray debug_trajectory_out;
+    3518       65275 :     debug_trajectory_out.header.stamp    = ros::Time::now();
+    3519       65275 :     debug_trajectory_out.header.frame_id = uav_state_.header.frame_id;
+    3520             : 
+    3521             :     {
+    3522      130550 :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    3523             : 
+    3524     2676275 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    3525             : 
+    3526     2611000 :         geometry_msgs::Pose newPose;
+    3527             : 
+    3528     2611000 :         newPose.position.x = predicted_trajectory_(i * MPC_N_STATES);
+    3529     2611000 :         newPose.position.y = predicted_trajectory_(i * MPC_N_STATES + 4);
+    3530     2611000 :         newPose.position.z = predicted_trajectory_(i * MPC_N_STATES + 8);
+    3531             : 
+    3532             :         try {
+    3533     2611000 :           newPose.orientation = mrs_lib::AttitudeConverter(0, 0, predicted_heading_trajectory_(i * MPC_N_STATES));
+    3534           0 :         } catch (...) {
+    3535           0 :           ROS_ERROR_THROTTLE(1.0, "[MpcTracker]: failed to fill orientation into debug print trajectory");
+    3536             :         }
+    3537             : 
+    3538     2611000 :         debug_trajectory_out.poses.push_back(newPose);
+    3539             :       }
+    3540             :     }
+    3541             : 
+    3542       65275 :     ph_predicted_trajectory_debugging_.publish(debug_trajectory_out);
+    3543             :   }
+    3544             : 
+    3545             :   //}
+    3546             : 
+    3547             :   /* publish full state prediction //{ */
+    3548             : 
+    3549             :   {
+    3550      130550 :     mrs_msgs::MpcPredictionFullState prediction_fs_out;
+    3551       65275 :     prediction_fs_out.header.stamp    = ros::Time::now();
+    3552       65275 :     prediction_fs_out.header.frame_id = uav_state_.header.frame_id;
+    3553             : 
+    3554       65275 :     ros::Time stamp = prediction_fs_out.header.stamp;
+    3555             : 
+    3556       65275 :     prediction_fs_out.input_id = trajectory_id;
+    3557             : 
+    3558             :     {
+    3559      130550 :       std::scoped_lock lock(mutex_predicted_trajectory_);
+    3560             : 
+    3561     2676275 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    3562             : 
+    3563     2611000 :         if (i == 0) {
+    3564       65275 :           stamp += ros::Duration(0.01);
+    3565             :         } else {
+    3566     2545725 :           stamp += ros::Duration(0.2);
+    3567             :         }
+    3568             : 
+    3569     2611000 :         prediction_fs_out.stamps.push_back(stamp);
+    3570             : 
+    3571             :         {  // position
+    3572     2611000 :           geometry_msgs::Point point;
+    3573             : 
+    3574     2611000 :           point.x = predicted_trajectory_(i * MPC_N_STATES);
+    3575     2611000 :           point.y = predicted_trajectory_(i * MPC_N_STATES + 4);
+    3576     2611000 :           point.z = predicted_trajectory_(i * MPC_N_STATES + 8);
+    3577             : 
+    3578     2611000 :           prediction_fs_out.position.push_back(point);
+    3579             :         }
+    3580             : 
+    3581             :         {  // velocity
+    3582     2611000 :           geometry_msgs::Vector3 vector;
+    3583             : 
+    3584     2611000 :           vector.x = predicted_trajectory_(i * MPC_N_STATES + 1);
+    3585     2611000 :           vector.y = predicted_trajectory_(i * MPC_N_STATES + 5);
+    3586     2611000 :           vector.z = predicted_trajectory_(i * MPC_N_STATES + 9);
+    3587             : 
+    3588     2611000 :           prediction_fs_out.velocity.push_back(vector);
+    3589             :         }
+    3590             : 
+    3591             :         {  // acceleration
+    3592     2611000 :           geometry_msgs::Vector3 vector3;
+    3593             : 
+    3594     2611000 :           vector3.x = predicted_trajectory_(i * MPC_N_STATES + 2);
+    3595     2611000 :           vector3.y = predicted_trajectory_(i * MPC_N_STATES + 6);
+    3596     2611000 :           vector3.z = predicted_trajectory_(i * MPC_N_STATES + 10);
+    3597             : 
+    3598     2611000 :           prediction_fs_out.acceleration.push_back(vector3);
+    3599             :         }
+    3600             : 
+    3601             :         {  // jerk
+    3602     2611000 :           geometry_msgs::Vector3 vector3;
+    3603             : 
+    3604     2611000 :           vector3.x = predicted_trajectory_(i * MPC_N_STATES + 3);
+    3605     2611000 :           vector3.y = predicted_trajectory_(i * MPC_N_STATES + 7);
+    3606     2611000 :           vector3.z = predicted_trajectory_(i * MPC_N_STATES + 11);
+    3607             : 
+    3608     2611000 :           prediction_fs_out.jerk.push_back(vector3);
+    3609             :         }
+    3610             : 
+    3611             :         {
+    3612             :           // heading
+    3613             : 
+    3614     2611000 :           prediction_fs_out.heading.push_back(predicted_heading_trajectory_(i * MPC_N_STATES));
+    3615     2611000 :           prediction_fs_out.heading_rate.push_back(predicted_heading_trajectory_(i * MPC_N_STATES + 1));
+    3616     2611000 :           prediction_fs_out.heading_acceleration.push_back(predicted_heading_trajectory_(i * MPC_N_STATES + 2));
+    3617     2611000 :           prediction_fs_out.heading_jerk.push_back(predicted_heading_trajectory_(i * MPC_N_STATES + 3));
+    3618             :         }
+    3619             :       }
+    3620             :     }
+    3621             : 
+    3622             :     {
+    3623      130550 :       std::scoped_lock lock(mutex_prediction_full_state_);
+    3624       65275 :       prediction_full_state_ = prediction_fs_out;
+    3625             :     }
+    3626             :   }
+    3627             : 
+    3628             :   //}
+    3629             : 
+    3630       65275 :   mpc_computed_ = true;
+    3631             : 
+    3632       65275 :   if (started_with_invalid) {
+    3633             : 
+    3634           4 :     mpc_result_invalid_ = false;
+    3635             : 
+    3636           4 :     ROS_INFO("[MpcTracker]: calculated the first MPC result after invalidation");
+    3637             :   }
+    3638             : }
+    3639             : 
+    3640             : //}
+    3641             : 
+    3642             : /* timerVelocityTracking() //{ */
+    3643             : 
+    3644         499 : void MpcTracker::timerVelocityTracking(const ros::TimerEvent& event) {
+    3645             : 
+    3646         499 :   if (!is_initialized_) {
+    3647           2 :     return;
+    3648             :   }
+    3649             : 
+    3650         499 :   if (!velocity_tracking_active_) {
+    3651           0 :     return;
+    3652             :   }
+    3653             : 
+    3654         998 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerVelocityTracking", int(30.0), 0.01, event);
+    3655             :   mrs_lib::ScopeTimer timer =
+    3656         998 :       mrs_lib::ScopeTimer("MpcTracker::timerVelocityTracking", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3657             : 
+    3658             :   // stop the timer when timeout
+    3659         499 :   if ((ros::Time::now() - velocity_reference_time_).toSec() > 0.5) {
+    3660             : 
+    3661           2 :     ROS_WARN_THROTTLE(1.0, "[MpcTracker]: velocity reference timeouted, hovering");
+    3662           2 :     timer_velocity_tracking_.stop();
+    3663             : 
+    3664           2 :     toggleHover(true);
+    3665             : 
+    3666           2 :     velocity_tracking_active_ = false;
+    3667             : 
+    3668           2 :     return;
+    3669             :   }
+    3670             : 
+    3671         994 :   auto [mpc_x, mpc_x_heading] = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_, mpc_x_heading_);
+    3672         497 :   auto velocity_reference     = mrs_lib::get_mutexed(mutex_velocity_reference_, velocity_reference_);
+    3673             : 
+    3674         994 :   mrs_msgs::TrajectoryReference trajectory;
+    3675             : 
+    3676         497 :   trajectory.fly_now         = true;
+    3677         497 :   trajectory.use_heading     = true;
+    3678         497 :   trajectory.dt              = 0.2;
+    3679         497 :   trajectory.header.stamp    = ros::Time::now();
+    3680         497 :   trajectory.header.frame_id = "";
+    3681             : 
+    3682         497 :   double x       = mpc_x(0, 0);
+    3683         497 :   double y       = mpc_x(4, 0);
+    3684         497 :   double z       = mpc_x(8, 0);
+    3685         497 :   double heading = mpc_x_heading(0, 0);
+    3686             : 
+    3687       25347 :   for (int i = 0; i < 50; i++) {
+    3688             : 
+    3689       24850 :     mrs_msgs::Reference reference;
+    3690       24850 :     reference.position.x = x;
+    3691       24850 :     reference.position.y = y;
+    3692       24850 :     reference.position.z = z;
+    3693       24850 :     reference.heading    = heading;
+    3694             : 
+    3695       24850 :     trajectory.points.push_back(reference);
+    3696             : 
+    3697       24850 :     x += velocity_reference.velocity.x * trajectory.dt;
+    3698       24850 :     y += velocity_reference.velocity.y * trajectory.dt;
+    3699       24850 :     z += velocity_reference.velocity.z * trajectory.dt;
+    3700             : 
+    3701       24850 :     if (velocity_reference.use_altitude) {
+    3702           0 :       z = velocity_reference.altitude;
+    3703             :     }
+    3704             : 
+    3705       24850 :     if (velocity_reference.use_heading_rate) {
+    3706       24850 :       heading += velocity_reference.heading_rate * trajectory.dt;
+    3707           0 :     } else if (velocity_reference.use_heading) {
+    3708           0 :       heading = velocity_reference.heading;
+    3709             :     }
+    3710             :   }
+    3711             : 
+    3712         994 :   auto [success, message, modified] = loadTrajectory(trajectory);
+    3713             : }
+    3714             : 
+    3715             : //}
+    3716             : 
+    3717             : /* //{ timerAvoidanceTrajectory() */
+    3718             : 
+    3719        3607 : void MpcTracker::timerAvoidanceTrajectory(const ros::TimerEvent& event) {
+    3720             : 
+    3721        3607 :   if (!is_active_) {
+    3722        1816 :     return;
+    3723             :   }
+    3724             : 
+    3725        1827 :   if (!is_initialized_) {
+    3726           0 :     return;
+    3727             :   }
+    3728             : 
+    3729        1827 :   if (!sh_estimation_diag_.hasMsg()) {
+    3730           0 :     return;
+    3731             :   } else {
+    3732             :     // we won't try to transform and publish the avoidance prediction if we cannot transform it
+    3733             : 
+    3734        1827 :     auto                     estimation_diag      = sh_estimation_diag_.getMsg();
+    3735        1827 :     std::vector<std::string> state_estimators = estimation_diag.get()->switchable_state_estimators;
+    3736             : 
+    3737        1827 :     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();
+    3738        1827 :     bool got_rtk_est = std::find(state_estimators.begin(), state_estimators.end(), "rtk") != state_estimators.end();
+    3739             : 
+    3740        1827 :     if (!got_gps_est && !got_rtk_est) {
+    3741          36 :       return;
+    3742             :     }
+    3743             :   }
+    3744             : 
+    3745        3582 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerAvoidanceTrajectory", _avoidance_trajectory_rate_, 0.1, event);
+    3746             :   mrs_lib::ScopeTimer timer =
+    3747        3582 :       mrs_lib::ScopeTimer("MpcTracker::timerAvoidanceTrajectory", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3748             : 
+    3749        1791 :   auto uav_state            = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+    3750        1791 :   auto predicted_trajectory = mrs_lib::get_mutexed(mutex_predicted_trajectory_, predicted_trajectory_);
+    3751             : 
+    3752        1791 :   if (future_was_predicted_) {
+    3753             : 
+    3754        1791 :     mrs_msgs::FutureTrajectory avoidance_trajectory;
+    3755             : 
+    3756             :     // fill last trajectory with initial data
+    3757        1791 :     avoidance_trajectory.stamp               = ros::Time::now();
+    3758        1791 :     avoidance_trajectory.uav_name            = _uav_name_;
+    3759        1791 :     avoidance_trajectory.priority            = avoidance_this_uav_priority_;
+    3760        1791 :     avoidance_trajectory.collision_avoidance = collision_avoidance_enabled_ && (uav_state.estimator_horizontal == "lat_gps" || uav_state.estimator_horizontal == "lat_rtk");
+    3761        1791 :     avoidance_trajectory.points.clear();
+    3762        1791 :     avoidance_trajectory.stamp               = ros::Time::now();
+    3763        1791 :     avoidance_trajectory.uav_name            = _uav_name_;
+    3764        1791 :     avoidance_trajectory.priority            = avoidance_this_uav_priority_;
+    3765        1791 :     avoidance_trajectory.collision_avoidance = collision_avoidance_enabled_;
+    3766             : 
+    3767        3582 :     auto res = common_handlers_->transformer->getTransform(uav_state.header.frame_id, "utm_origin", ros::Time::now());
+    3768             : 
+    3769        1791 :     if (!res) {
+    3770             : 
+    3771           0 :       std::string message = "[MpcTracker]: can not transform predicted future to utm_origin";
+    3772           0 :       ROS_WARN_STREAM_ONCE(message);
+    3773           0 :       ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    3774           0 :       return;
+    3775             : 
+    3776             :     } else {
+    3777             : 
+    3778        3582 :       geometry_msgs::TransformStamped tf = res.value();
+    3779             : 
+    3780       73431 :       for (int i = 0; i < MPC_HORIZON_LENGTH; i++) {
+    3781             : 
+    3782             :         // original point
+    3783      143280 :         geometry_msgs::PoseStamped original_point;
+    3784             : 
+    3785       71640 :         original_point.header.stamp    = ros::Time::now();
+    3786       71640 :         original_point.header.frame_id = uav_state.header.frame_id;
+    3787             : 
+    3788       71640 :         original_point.pose.position.x = predicted_trajectory(i * MPC_N_STATES);
+    3789       71640 :         original_point.pose.position.y = predicted_trajectory(i * MPC_N_STATES + 4);
+    3790       71640 :         original_point.pose.position.z = predicted_trajectory(i * MPC_N_STATES + 8);
+    3791             : 
+    3792       71640 :         original_point.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+    3793             : 
+    3794      143280 :         auto res = common_handlers_->transformer->transform(original_point, tf);
+    3795             : 
+    3796       71640 :         if (res) {
+    3797             : 
+    3798       71640 :           mrs_msgs::FuturePoint new_point;
+    3799             : 
+    3800       71640 :           new_point.x = res.value().pose.position.x;
+    3801       71640 :           new_point.y = res.value().pose.position.y;
+    3802       71640 :           new_point.z = res.value().pose.position.z;
+    3803             : 
+    3804       71640 :           avoidance_trajectory.points.push_back(new_point);
+    3805             : 
+    3806             :         } else {
+    3807             : 
+    3808           0 :           std::string message = "[MpcTracker]: can not transform a point of a future trajectory";
+    3809           0 :           ROS_WARN_STREAM_ONCE(message);
+    3810           0 :           ROS_DEBUG_STREAM_THROTTLE(1.0, message);
+    3811             :         }
+    3812             :       }
+    3813             :     }
+    3814             : 
+    3815        1791 :     ph_avoidance_trajectory_.publish(avoidance_trajectory);
+    3816             :   }
+    3817             : }
+    3818             : 
+    3819             : //}
+    3820             : 
+    3821             : /* timerHover() //{ */
+    3822             : 
+    3823          82 : void MpcTracker::timerHover(const ros::TimerEvent& event) {
+    3824             : 
+    3825         164 :   MatrixXd mpc_x = mrs_lib::get_mutexed(mutex_mpc_x_, mpc_x_);
+    3826             : 
+    3827         246 :   mrs_lib::Routine    profiler_routine = profiler.createRoutine("timerHover", 10, 0.01, event);
+    3828         246 :   mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MpcTracker::timerHover", common_handlers_->scope_timer.logger, common_handlers_->scope_timer.enabled);
+    3829             : 
+    3830          82 :   setRelativeGoal(0, 0, 0, 0, false);
+    3831             : 
+    3832          82 :   if (fabs(mpc_x(1, 0)) < 0.1 && fabs(mpc_x(5, 0)) < 0.1 && fabs(mpc_x(9, 0)) < 0.1) {
+    3833             : 
+    3834          47 :     toggleHover(false);
+    3835             : 
+    3836          47 :     ROS_INFO("[MpcTracker]: timerHover: speed is low, stopping hover timer");
+    3837             :   }
+    3838          82 : }
+    3839             : 
+    3840             : //}
+    3841             : 
+    3842             : }  // namespace mpc_tracker
+    3843             : 
+    3844             : }  // namespace mrs_uav_trackers
+    3845             : 
+    3846             : #include <pluginlib/class_list_macros.h>
+    3847          82 : 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..38c674450b --- /dev/null +++ b/mrs_uav_trackers/src/mpc_tracker/mpc_tracker.cpp.gcov.overview.html @@ -0,0 +1,982 @@ + + + + + + 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 + + +
+ 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..5541b3db44d56a3ccc412a4ada79e6a52a1042a1 GIT binary patch literal 12412 zcmV-?FoVyDP)vVxgI*zH%Ih3Y*{;uOWmvx11h^-m#Q*#R1fSs zBc=nF!#G}gpoo_MhxZJ+uHc@~^3j2=UvW@U)+ zz+irgfOiDgHO4ikUBVu>w+H|iDc}Vu09@S|duJvL&Y{&Hl=dHotQfBZW1+d>)d5#E zAJf@bmOjD&xLh0;O@PZzTDz=}7zeFqfbkwb+@0ONIM+!LaIxNg^OXd=ek#J~ z0Sg$Dr9f|+L1czWZ=AvF^t|5T7|Vc>Xb$7|H!_S4ki#e^X7OA5>d$EoWqvvXc67zr zJQJ`03l>-REIW*WJ%-ndKN-8}!CepbA;Xin{j!o4FOutfVGKVAn8K24*YIq~CHzhS zwoCYmwwclvm%9*_u0sZxy$T@1!)$vGm}(>ij%6hE5VKmLEFTrW$FsnZrBe*YxIhY) zOz8wbwETIarv=b=%>b_0RR1Ps@k%$i{dH%BMYvy^E+wk{?u?J>^m>u*5rYJy-7PL2 zls)5^n0Q#=0nG=G|L2HV1#=S?9$Hw0F_{Az+#Ob@S1DB#h>!OIQ7Iq`3>jWSDkLUp z>;{_tM0Rk{bPY(tnEL_CTc}`+{v6zYSg{-0!0W5b2aq`VyL|u@AKwUgS8~q$#ZbF2 z45h}|l(;(Jh~Zg`3vdR7opSL%X%h;goX2^5q2Mm~OY0cpolpgk)S|@S(UBaI9jY+K z)mkhS6!)Og*77}ITpg`rA~2?8F9aVn7^Myph_<-X@qoDnKm!t8hh9oonfUb7GR6)c z=@R}IkHU*T4+F%+6gGDnx()`WJ&8xx2O@~*x|3|O>-Fx>L-t<-NZp4GrFuogd;iZ$ zb=}o3?%HYZv`-I6P~B%2rgpUyaD=}{b-wm9gP;;U&KeiZW*EGSvjI+iAokN@rII|R zblQIY+_na9l;8AHz-$o`QnILYc&&NADA#2eA_6RRsdm#7?4c15I*L+Sny%~3M;6fR zf!)d2bHiJVWQ+^ydMea&US8=|_8 zVuKM;9ac>*)!{?&Y6$+to!0;vwVcEf7<-+msY3&EsKXN%{TjHfQa~xjT-(rqz3lN9 zt7w)Q*t7Hvsf@97&z2~j>Xn?L>&07FDp_6K^aq#Jxf3ic+{(L$!2;|VWikhF#b+%fOpFC!2qT>^TFev@_-L4D_ z4`K^sQ~-|vNDf5X^^SN(p==;+h1)#j^H&T7IAZ9F43P5Ss_D0;_ z>a}THU^0zYQpdZOf?@VIr!TG-XT|_-Qoz&Bk#Pn}%{0cddNLWF>IU8mSMs^u61yNecp3ZuuC&*Ogn0N@F| zb`?NE*7m-GcsfI!dtoYQNdsN8fQPQ@P!!NQT6h-=URrxX>lq{pGu(dJt=2h^CoNS{ z2}O`ST4H>RGD}acCw58?IXBLcZw;v?Qq1)hIrB~Oh{G9-1upr>uzF0UuuVz9U314A zJQ^`lI-CcT9PI^n1)Jfcba&kZ{hF?2JuA9~Y1PfaM3@+A4bps+5)*GaK?Q-HYqBkq z1zP=4>skVMh%pubrR*W(`yh5O+Qy}{l>Q>8Iw_!QBf?p)ZXD?t^Wg=+CiYOg4uDFk z{TDuPHDfrl$#%^3elm+Iu=;Ae;{})s4iNnz*EO6M7SPW$qqOp_tAGw8rv895@5X)-^0={}Xgq_7Ic8ZYf#!iMGlx zF1zv(w#{*##vQd`d_2CMuMq6^5m4x1_da`oj&#mXiXt&Q zB-Ho;)|y78z##S^0x4BPQCrt+3aN|aVFG?$RknNSiQ?lv&~q)5o_7jGP>`idhych) z&=K9UQt1Kfx_IxRa#Xqk8o}KhHaS&BGZZsX0HinAGcRv$7RmvSvcB_x4=ZO^s2XgP z;EDlda@NnsEROW8?D;9GKR|jYZ6*f%|4Yx?s3tUJ9OLu(^;H9 z*B(o@6hXTHMa$4ixWL|HyQTnTU7IRKk~}(L&u`T|1))f9S4w3qJ0M!L{tEs*|6|)r}d%Qrhm&n7}LtgKbSLecZ zxkwJ4dk8S?nNFF|U39PRzdIPkLDmy0Q$hyg)i5fLAE8`^gs*M~FZ zX3M=o4N#BKPV)$iZ_YV;OXAm7<)9v;___eE89s_+ilzumEXd_Y-AYZY{lMyQDSNi+ zEVp300b|z~yY_2pVKiDP1ygSFHazYUIh~wg7|YHpu>ufxJy8J+dk+5V zEAcxrE=I+@Ys6S|`4II0t_-W+m3ypm`04z_@6hK+~e-iVkUm^!U2}a}C9|kCEP)UK=VP~e!%PD|rVuHpXbz39G7prY;H z4*%_LC8;HFw;~-_9C2Pu5*&iWmc5i$kI@|+3wFSkIdaqd9cql@0UcAQTW13BB(JAi z*t1sZd{l`EX$VsR-@=}?>Xb^l-ZzQ0b0+lyjWWq5U(Js-yd%37QBToTSzIm*o$W~9@l$JnJzj?ep~ zX9M@r<&2yOcF*W!AR;H_umaC zXcDT0#7JXZYeM1cr%aM?;fcM-6#ys`o}{8=&?w%$=dXxf6O&H8QlaYOng3ux;*+en z=bFwt1B%b_#|xMkTV^;Yz*6FUswr65kZAMboXl}okShBL_l1gZ)?R zT$M9L+q!lbV=#1*?v^es5#ZJOG3to{zZpRKXElbWTIlt$H*^Y{`z2S%hd^3*c zS~`U-$Gz(h>Eqs^JMO`UlAWWSB)c~%o~p_rWY2b{h37fX#|*%HN+^x{BLVqTCmrGQ zZYwMSx|wug7s;Jyb1uMn(xEf@HRJv?VzaR-A!21mfCuH{c94lL#RHlsj2Js`G(S|6 z^&Hu$!B~jIL4ZwTMB_Y)fC(#`Jeu_a{#uOCDV5=lTxzlQ^0Bs$k#0aA9Yw(A$8>m% zh-W_x2t`Tf4g+t?`WI&J-hZht&6)aAz~?F-Aq^FV6EA9XP9Y)fQviF$m{kT)J@``; zv$zx|C!H2CepJ%wFnc%&zjafa|Da(*8eIw%2L7?NnuHkPtvwV#0?^qGJB%K)i{Xen z(mPM=aEyoLJLFW@PiM#N9i@5zkZq-|BjCuCW$FSu{n@7|fLOYENL;>10i5k;1%4{i z^z;ItqHCiW^6l?%3l#vy)j?9P@46e$Lh0R zR%cGFK8eF846LOOu4~wqQx!kbM5)x7F_1?IvPN zxjNo$SoDN~(Nb5afn2Po?Lo#FGbnwxj3#wRrz-vw~&C>A7B0d66K zq%VPHbq}XH;x1}9|K_dCT`Nb!XF<;@KJ6*3LEfAl>>J?)vu(cy^jDg{9tAXNq>=&qM8 zE+tDud{N(b0g~6*1fYZc!kR->G+D8>h4Prk#LyuzVtL#mn!>{NfTg#X&7ZV%eWppy zCsZ8GfNVNs7Mi|0p9=vvIyBu^HxSfu5lRm=g(;W(%vd5Tljs4DY_@$$bizjsVA-$u zNes`a&MxgVfcxQYIK4dXZes%x-tsXj8emVEL0a~d4@jFDuv#p7vh=KhO(fNEt?dO* zDF!63;XU9ShwlV0m&}nVc3vrn37kBggC-}i?MD1JS78MmA18C{`gc9-%gAW6tE?mO zC`k5`r^dz&e^moiV|j$lF)ZThKy`#aaQpjPhE2Ghn?}s$MF_C^)cxeNs`U z=&tYXL4Jo6e>?1MI2PZ?ylkx)5fv4{<4Ahy0o07~|E6){%~Cf;nZ{Ll6@0nYFTzdG ztWuO}ujYRg18Tt1 zs&mppm9wbnG+eH3!d{3aN_F-PBqG&=XOmTD$ubnNAp`f*G%y`cd!fCg9!c+>$MUsAxg1=&P#EY}!m z+GmPSqP5-)pKRu2Pp*`~nvGQv>xu+`yk}|{W9bSfNY7{Og`)~pGkamev9?sQk#Wt1 z;l(Lz#z@F41k?&{K?jTw+~l_TvjlgP@D_|X#I+wb;;@V4eGjbg22y^VDr66nBT4}3 zF@`Ol&Z?YQj<7i4jqC~AYy8lOkz_Rs*qvQ~Pq~GBg~^w8K!mfdp(qxK!7@80`J!M` zM7{KZs(FNNR-CUR%SwMD?|SmJi@}_YRF9G23kSHBeM1d^mHf1S*1X0m!GG~T(07=+ z;`^7E%qW~^_Xtdyna-;%(dHS|kRb(f3hoEY&<0I0Hq$ONyL(aFm(FG9JaO&~jk|l0 z>K=20F25YXW@e0vIfVWrAm|fxi4^mD- zW7kXym<3Y@K;h#04a+r%e-h-XW;j{k(8&Uv*=F;aafbC6?D#FJ$$*Vx%%hPHebw~Z zdfi=>7&*qhmeu3{AvMVmj>&*TDgiY{aeL4@1u7Mj)0V$9z~~eLkk%=xloOFod1q13 zL64%x`PSYcvA1AU*VJl>smAj2^@LFO>2WFjK?Cg6B|mO3xqi;jRNuQ69r_ik*;>}EdKa~{!r+C)LNv1)k>e(v&5ZEQCUem#-CV)y{FP*L__q9 zH-&xMZHuvm8>7Qs;7fSd+chmlw^REqn%MOe(cGG@|4%RPdLbNisacD#@xRqDJU*j6 zD9&DChiDLk!LoO!<@j!mb{N*?#I8YQhvzm znzl|Oj<6rJ7>$cy+}#H-37M)>+vnIl#kNL3{CD>xOc4MIF)GIBQ7-WQ!Cyl3T|f+r zp3Nc(&R8QW)F;!_OeGfAeuhGiE%80>T1o&Y#5kpxrR7+4jQ?XFz>wQ!N;T&)`6rL% z=IzJ&OQ%x1=L6+&fJnN_HK)WwY<$q@?&9lyJ0;$$<7nBhGrSOEW1wU)K?Oy9vWfOv zqwzt+hQxay6~+VBHirCZ8{;+GL_zmlN;h=Lj(Tp5RutAZ(hviLH{8vxL25w;1o4U( zq-CcrX!jiOa3w-@V+0q;d^HUN4vYK{)h;4?Vm%DMX%xE-n!+>2*=#H5nwVd*YZcYY z8q|kNN^##9>9N#e6hpn7MU<`nuE>?O5v%n;>F5;QYH z?ko=UPtHe1|4h?8XcH%|iZh0DC7b3jd@$Tah=`Aqqq!@0ak8$NpoYw{(VGe((#pwE zc+$*SjA@*`EptZbQvK-zFA_wT9wW{iWg-YQ0aeKD8mnYxUe|X%H=0=195Ejj>l$oV zV9oT#x(=x-1qAF&c@KYF5wkr8bBddA{i_4X&3N*hWgt!9bvGuc%KcP%lp>9+IDkV3 zDDCH?`~+fVC;*+ZcLVi>j*n!(8bGaTMi|e)SN{VI z;M9*HP z11LSkNt^FEc6l8lzlnS1PZG$weuSv6SMeR1NcHP0>n;KOJ!jWkAdOUg$k`PfREHS% zC&iuvamd6qXpeif=lRqJ$e~Blw;rVs$GL-MOD3|t>M0Z&mt%bAaZieoKhLyd47OR# znVR>VA2*HFEU`Xlj7MH%af%&2tzsH?(zFN7pPU&g{jzgr2;C!{nF$a63_f4q{$Gc) zUvi$=@te|3S(PNlxU2&1cb-15$yHdcpJsB6a_(nw3hZ3V*rR)sMD(`HJFhC=I7T-} z`-g0SKo98kP7dgPtF@W|uH53?lVdXnc(^fE0WEL>NWC{?`%W=tkF5D70;Dj~UtxcE@vj1G@$9T1F&<9$v6wd;OjNt=@j&gD2(f{avNtlm;7~vQ*e@2$OklDCZs(qN3 z4+Hj5{wdXp$CFXOkU!h!#B6pFq5p^r$bXjy$>%@#JPy|LlUWMP8DPRTEN-$f&N@Jz zF<=LGhv&K<5Cc~LL8%t;%;0+k4&Nw12wVk(@jXgnQehYmm;jdn+iKbU--y9j=rp>< z?E|Xu_9BerAjy)6D5J{0fNZ|R7m-gJxO-YtbC~-f$-E#hy+f#5<&A8M@$q+*fywe7V
  • ^(Lq6Q;{JVpa2}s9&gwO%l?{qnvdtn zD!?Q#}c&OvO+BwqG#)aeDlm?0S9@E+BDPE-v7jwTd;|F z?bG7<>jDAbyx5r-IZhZWnm}?;rFx}Y_4-z`QQXf}cX4t{O4Rq9Bfy_cF@TSsXHyIT zR1!nXMG=ty1J0%(v>5^9=jZ&hDL%xe_)}5^_UL8?PtTu{N->h_qyWhO!CRG7Z%9mv z`JK9FvPual&~H{Ln4mE#1#UQ25b>uB2&N$u3VbI30sY_7{>cVD{^a6v zO+Q9A9wt_>VT>xfCk6}@sg@G6!41yb&CvSsQHtC(`Tnn$4cxVW4|o4uy3>4wi|R3# zpO_f)52+8DiK%Dr&m!h;X8va8Z)R%5$hTo(#uBEwFp88tC8|I3QA?6NMdo3^GhT<6 z?}mNP{+5;loEUJ54tyZg*moVhvxl>O!f4@l%yZ0UwY+?|((@^O)9!nL$p~4bStbT> zO3HHN4?o*2rw^Yo~yw3seiom@n6zn832jZa>=`Y`2(+SEG7A&OKZvShBBwZMXgh zxA=2=s&V320<=H3oD={Gy2gO%pi3ChpzB?`LHCdG*8vLJ&Hnkh1r!Cq3wAu<`M$0d zfM+=m>Q{`>%YGTK-hweqs0$8bw(^RAn*}6OB?!QB*X0I@sRH`0#{){Hu-+8g@OZOO zdy!MlL~yB%LO>OJcF&M~w+`&=Yz_e5qg)0s zpvcetf1`T*O~sZtr74;jstKaMGOB5sf6rY1J5=BN8<|-HSy1kvcfX~Nh_1ms?i8#& z&ATH#^~!w28Zj9II2IA@$uhWGyf*l@%WN2 zn)Ws0O0;B)=8x+4Xo`_mO7EI6{CR~Re7!zy&HS5GH>{bA=+9U)|2Wl^%z>2ZVpt;% z`(i*f#JnvOa1(|CYJJVu$DN2g$^fI@;ehpS?ng^~2LJ#M{z*hZRFib1BEZpQP=u~# z1u;_}^Y#zSgSWgpAM+j*T7c8o3lHjGsXb7J3;fMHLlBIO68ts=25n9~_Xi?LFi4@daMhnJ6f0@u?W3YeW79XU1!u`ntJ`vRQh3J}i-;*4pSGY#q7H?DD7osI@ z8&!;hBu3cq!(g$Dc89V|3XPj6(!0 z&?S7+-+NIE(W=>I;;uJPQ4YmkHe(E%++r#~hP{cHn-GG$I+agDLo+;VUvLK8V`)ux zkKG&CoC-PBi0sntHSI-2PfYg7E=w&jKBr*yapG=0VlCnT?f=3bIYyu{?s$~sh9e*2 zrA-M99zJ{JuX&glV|+5_l=mJ}+!}H7V#}UP7FF&8V2mygST(xj>-sh!7?_zFsvuoB zpp|sNmQGEW8NAc0)=cuuDkBvSFf}jihH7Lu#25qv=;g1kSdm;jg@fnNFbS(EUO!4q zaWh0==fih}i#OhO8iFukj@M>CQR;8TF@@IP=9@h#@iL%G+0bnt;pH2sC05G$6MaoJ z_5i0;ak??!h$rTuo?F-86YKC-SQBQ6|G57*Bg7$^R$@~lz{Y4!2RsS;Ll+)OEHUWI&!yrGm z6w+r*+m0!v@lFODawLwI?4wGi3@Gf3Za`X(0K{7aU=_+SQZ7$7wA1&{oUs@Y=LJDf zUmJ)BXe^J3oMvU0UD7jMFlnC+c(Ro@cNN1^9Zo9qW*wdlaxS2JjLDTR3z(N+$hmHd zzlgTsM`l3c1Asr(y#KPvKgHrKlZ7}~WNbX?V`rQ6l=j(%>nmtwE%8>B8%q#B#j1jMiX#Eom2V;z3CY09| zF#Hwfg^&s*!^d&k5d)SBsJ)oPu+IS$twM~^AtjpGT}2o*?Olb-BF%QtwBefBxcr-= z5%7o@%YY8L4!Gel647CBgoT;;1ji5*Z%42H>~8~i-I*&XxCw0ze>bl^QI0QC`;N+- zrfH10-&L8DxQ%O6dowF@QZq9HP(Q}M%A5%|J6t3<@d2}^1>;VlYsGi~Zw3)Xg6cog z-V7%c5Ca96VswD@F?x)AC~zajfq;hggz6(cUG=ew^f+==+N@Ge&IHk8OkQsNDL~a2 zVPgRsFiztmQ9N}LplXZ~KK`QZgC|Bvb%v3Y%TF`d2c_h}9G@e8&DMlGjrD!pfk~$> zc5eVi9|(AN@r~|sECD2T4aZ00S_%K^X2#4CCEek=?I8#$nned_3R^hmnNQWNe3rxKfg7l{7{cqLFb+?IGe2Cf%K;;;r0w7BWG$??2sg|#JFI0TA z?c)_dC&vGU0%&w8b1k#Ax^qb{!8JuEP!Q9�GnV?etT#pCaO`05)JOMfxi+`sxLH z^5$?y2tvxoBh-rN$5?v(t`TDqPo|Tix6vF!sA=+m9)9>}3^@yOq8GrIFIoCRYH$c46ETKab?<9Hfc5KX2+f z^)+=8Gd}MFWW|6i?pI9ac6AD~%1TUDOhcL$NS`5XRJrN;8S(g7HTS~AdFESM~(4;n3rVmHDY{{7+)_G=clW9ArguU8Vd%^t>z*+Zt;`p zZlepAK9W)-Pdp*0YdpY0?w*)xj&&KJ2ICVAun*(hWJ$jjgfG_D5Q@J{VR(I8_=v1Z zmW|2w`nS7|!nh3HTxGlKQ@0m}Ngltu2HbpnxCkSu>edX z2>~iZt222QEQF2AVn1xfw0VFhn-{Okpw0jIYJKd9tq>NUbe-k__jT<;5LmzlVu~o+ z84gebt`UIO4~fvKj;>K!_c0f+nLSJj%{#5V5!Fo^Z<}3gsx5~diuB#{M-q|ok#%kv zyX*M48N(w@&*+Ivr3xz$C^hVfm5W2cVyaTyhWgGFY50Ndn&cmzMh6IzzAs)eR1dGX zE0)KtR)5it@om+g{1!LzMH@KL{P`?DA%XPR1N0x?c0pq!G3NX0?xX`YD!wQ0*HmB` zp06Md(w21$HEwYeZk$3qLG2hrGWih&j>uAd=Xr)1BSFjrp`a5R()FWaxRGa5P=U#s zxHP}r75tkgWsH$Q)Nmo7x@yt&s1jAa^ySZmZ>w4~|#RD)-rosh%i318xd!Bcz9vpBd8Qf$SfW`sxB0BWCWk>}OFZf)CP_S_WO`Y=Q&+ zEa@3{<3k)+hq>&W8AUe$NcdV1prY&4I0={$;D@Bz4vwj2GH6ei_Y$Uk>Rgv{wOXC2 zpl$^LsKAJ_{~|fKR+ZwI65iHY?*47Nybkg$Jpw=r8?Qy(^%b6C7Enq|d|b9HW{>2> zrl@y2A^|h4O@-vWGlFdsKXW9XLM-Dhg$77s0DPwhiEXIH`Da=7bXy}J`n&8QO8_Xu zs2O8Zh1nv;aF@80-pi%}3WBJgZQDom78U```j=F_6e|EwF%d4;tN{tn3+Q4%In@zm z5%5E-lx3VOUM&$bQp7ADj&noZ(B^iHA_n)c4zX4Ooa7 zYBUSsn#lm=7&YnX@0uBK_wj4T7-k~OIHA=Kk=na{suZ)s+-&mY`!Jvc<4N5lWPtY9 zv=jnBc@vhir=0tpuT{IEWnucb4=Et~*#d6pjKi_#B5#oxFTK%<3D=se<_!jMP#4|v z0MAmXpVlM5mIkH#9AHXMgl!?FrT{$P5h9xU9}q};Cb3S4OclNrp?WNWbyT2_J>kkU zi*y(zo+0g>=J=<<_Dn47H?gXQ4rB5{2E-R@ypRE3 z{eHT)-#s(|D6x^`iz|+?_(BF4a^vi8OUrW2QL!xOBO z(XOF+AzJ0criQ?@id8r{60kN#DKYHu+c6#+`5Xt?^}bi%^HtrPLF4%DF3&=pV12ra zAETY?@F8QG+9$Qw;@z-Y@fn?quMnqU%%SH5e~*7H=6?6@j%xXqHGM#SvSf3i$d_~L zBQVM-{AF(!PzM?nM&z{{S93q-G;t$d1axKfb%3HVUQMJ9Hq0O5=?mMe!EV+iS<`?x z0wm!?PkVR>^-XGKs9oV(ymt%_E52m zHrzLB>FTC%OV?ibMhdvc&^INZW{fqKPnhf0nK5F?6QiQKV4=(eRE;rj`Du9C%#`OQ z9pj!3uAS5~9Xnwc6vu}W(dFuGb4sH#-``O9z9rw=$h(#Rx=}^DyNHp#!XW^WVox?IwPa5}(GO7*RKx9D86f?Nn72p; zd!ex1exkZbNC)UhZ5$gCbGtcz^IFMG-BN16-rh_M#>luor~nqcE}HH%3m9>AWdY!> z18f~56@RIwx}sDLWwwh^Dz*5h=G$i0@zR1ZW#7*L>c&XTNDIc>aD8U9WJ%p$?&A86}LDu9Z9r0Ifj`Iqj9%H!gvD=@&479#)`Qj2g0mWk+zqnku u;M!RAtcGz&v{WpE78?mj%YHhm=lLJmPUiG@;u^gG0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trackers/src/speed_trackerHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:6141114.8 %
    Date:2024-04-09 22:03:33Functions:71936.8 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

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

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

    Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
    speed_tracker.cpp +
    14.8%14.8%
    +     		14.8 %61 / 41136.8 %7 / 19
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func-sort-c.html b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func-sort-c.html new file mode 100644 index 0000000000..9f7baa578f --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func-sort-c.html @@ -0,0 +1,156 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trackers/src/speed_tracker - speed_tracker.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:6141114.8 %
    Date:2024-04-09 22:03:33Functions: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&)4
    mrs_uav_trackers::speed_tracker::SpeedTracker::deactivate()18
    (anonymous namespace)::ProxyExec0::ProxyExec0()82
    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>)82
    mrs_uav_trackers::speed_tracker::SpeedTracker::setConstraints(boost::shared_ptr<mrs_msgs::DynamicsConstraintsSrvRequest_<std::allocator<void> > const> const&)255
    mrs_uav_trackers::speed_tracker::SpeedTracker::enableCallbacks(boost::shared_ptr<std_srvs::SetBoolRequest_<std::allocator<void> > const> const&)281
    mrs_uav_trackers::speed_tracker::SpeedTracker::update(mrs_msgs::UavState_<std::allocator<void> > const&, mrs_uav_managers::Controller::ControlOutput const&)111592
    +
    +
    + + + +
    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..3d08a20347 --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.func.html @@ -0,0 +1,156 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trackers/src/speed_tracker - speed_tracker.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:6141114.8 %
    Date:2024-04-09 22:03:33Functions:71936.8 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    (anonymous namespace)::ProxyExec0::ProxyExec0()82
    mrs_uav_trackers::speed_tracker::SpeedTracker::deactivate()18
    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>)82
    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&)255
    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&)281
    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&)4
    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&)111592
    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..d7b79e53c1 --- /dev/null +++ b/mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp.gcov.html @@ -0,0 +1,1178 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trackers/src/speed_tracker/speed_tracker.cpp + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trackers/src/speed_tracker - speed_tracker.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:6141114.8 %
    Date:2024-04-09 22:03:33Functions:71936.8 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

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

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

    Function Name Sort by function nameHit count Sort by hit count
    eth_mav_msgs::yawFromQuaternion(Eigen::Quaternion<double, 0> const&)849
    eth_mav_msgs::quaternionFromYaw(double)2257
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.func.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.func.html new file mode 100644 index 0000000000..3ccabf3e3c --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.func.html @@ -0,0 +1,88 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgs - common.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:44100.0 %
    Date:2024-04-09 22:03:33Functions:22100.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_mav_msgs::quaternionFromYaw(double)2257
    eth_mav_msgs::yawFromQuaternion(Eigen::Quaternion<double, 0> const&)849
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.frameset.html new file mode 100644 index 0000000000..1d6c346fa1 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.html new file mode 100644 index 0000000000..82be558b60 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.html @@ -0,0 +1,496 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgs - common.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:44100.0 %
    Date:2024-04-09 22:03:33Functions:22100.0 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright 2015 Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  *     http://www.apache.org/licenses/LICENSE-2.0
+      13             : 
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : // Common conversion functions between geometry messages, Eigen types, and yaw.
+      22             : 
+      23             : #ifndef ETH_MAV_MSGS_COMMON_H
+      24             : #define ETH_MAV_MSGS_COMMON_H
+      25             : 
+      26             : #include <geometry_msgs/Point.h>
+      27             : #include <geometry_msgs/Quaternion.h>
+      28             : #include <geometry_msgs/Vector3.h>
+      29             : #include <Eigen/Geometry>
+      30             : #include <boost/algorithm/clamp.hpp>
+      31             : 
+      32             : namespace eth_mav_msgs
+      33             : {
+      34             : 
+      35             : const double kSmallValueCheck         = 1.e-6;
+      36             : const double kNumNanosecondsPerSecond = 1.e9;
+      37             : 
+      38             : /* MagnitudeOfGravity() //{ */
+      39             : 
+      40             : /// Magnitude of Earth's gravitational field at specific height [m] and latitude
+      41             : /// [rad] (from wikipedia).
+      42             : inline double MagnitudeOfGravity(const double height, const double latitude_radians) {
+      43             :   // gravity calculation constants
+      44             :   const double kGravity_0 = 9.780327;
+      45             :   const double kGravity_a = 0.0053024;
+      46             :   const double kGravity_b = 0.0000058;
+      47             :   const double kGravity_c = 3.155 * 1e-7;
+      48             : 
+      49             :   double sin_squared_latitude       = std::sin(latitude_radians) * std::sin(latitude_radians);
+      50             :   double sin_squared_twice_latitude = std::sin(2 * latitude_radians) * std::sin(2 * latitude_radians);
+      51             :   return kGravity_0 * ((1 + kGravity_a * sin_squared_latitude - kGravity_b * sin_squared_twice_latitude) - kGravity_c * height);
+      52             : }
+      53             : 
+      54             : //}
+      55             : 
+      56             : /* vector3FromMsg() //{ */
+      57             : 
+      58             : inline Eigen::Vector3d vector3FromMsg(const geometry_msgs::Vector3& msg) {
+      59             :   return Eigen::Vector3d(msg.x, msg.y, msg.z);
+      60             : }
+      61             : 
+      62             : //}
+      63             : 
+      64             : /* vector3FromPointMsg() //{ */
+      65             : 
+      66             : inline Eigen::Vector3d vector3FromPointMsg(const geometry_msgs::Point& msg) {
+      67             :   return Eigen::Vector3d(msg.x, msg.y, msg.z);
+      68             : }
+      69             : 
+      70             : //}
+      71             : 
+      72             : /* quaternionFromMsg() //{ */
+      73             : 
+      74             : inline Eigen::Quaterniond quaternionFromMsg(const geometry_msgs::Quaternion& msg) {
+      75             :   // Make sure this always returns a valid Quaternion, even if the message was
+      76             :   // uninitialized or only approximately set.
+      77             :   Eigen::Quaterniond quaternion(msg.w, msg.x, msg.y, msg.z);
+      78             :   if (quaternion.norm() < std::numeric_limits<double>::epsilon()) {
+      79             :     quaternion.setIdentity();
+      80             :   } else {
+      81             :     quaternion.normalize();
+      82             :   }
+      83             :   return quaternion;
+      84             : }
+      85             : 
+      86             : //}
+      87             : 
+      88             : /* vectorEigenToMsg() //{ */
+      89             : 
+      90             : inline void vectorEigenToMsg(const Eigen::Vector3d& eigen, geometry_msgs::Vector3* msg) {
+      91             :   assert(msg != NULL);
+      92             :   msg->x = eigen.x();
+      93             :   msg->y = eigen.y();
+      94             :   msg->z = eigen.z();
+      95             : }
+      96             : 
+      97             : //}
+      98             : 
+      99             : /* pointEigenToMsg() //{ */
+     100             : 
+     101             : inline void pointEigenToMsg(const Eigen::Vector3d& eigen, geometry_msgs::Point* msg) {
+     102             :   assert(msg != NULL);
+     103             :   msg->x = eigen.x();
+     104             :   msg->y = eigen.y();
+     105             :   msg->z = eigen.z();
+     106             : }
+     107             : 
+     108             : //}
+     109             : 
+     110             : /* quaternionEigenToMsg() //{ */
+     111             : 
+     112             : inline void quaternionEigenToMsg(const Eigen::Quaterniond& eigen, geometry_msgs::Quaternion* msg) {
+     113             :   assert(msg != NULL);
+     114             :   msg->x = eigen.x();
+     115             :   msg->y = eigen.y();
+     116             :   msg->z = eigen.z();
+     117             :   msg->w = eigen.w();
+     118             : }
+     119             : 
+     120             : //}
+     121             : 
+     122             : /* yawFromQuaternion() //{ */
+     123             : 
+     124             : /**
+     125             :  * \brief Extracts the yaw part from a quaternion, using RPY / euler (z-y'-z'')
+     126             :  * angles.
+     127             :  * RPY rotates about the fixed axes in the order x-y-z,
+     128             :  * which is the same as euler angles in the order z-y'-x''.
+     129             :  */
+     130         849 : inline double yawFromQuaternion(const Eigen::Quaterniond& q) {
+     131         849 :   return std::atan2(2.0 * (q.w() * q.z() + q.x() * q.y()), 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z()));
+     132             : }
+     133             : 
+     134             : //}
+     135             : 
+     136             : /* quaternionFromYaw() //{ */
+     137             : 
+     138        2257 : inline Eigen::Quaterniond quaternionFromYaw(double yaw) {
+     139        4514 :   return Eigen::Quaterniond(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()));
+     140             : }
+     141             : 
+     142             : //}
+     143             : 
+     144             : /* setQuaternionMsgFromYaw() //{ */
+     145             : 
+     146             : inline void setQuaternionMsgFromYaw(double yaw, geometry_msgs::Quaternion* msg) {
+     147             :   assert(msg != NULL);
+     148             :   Eigen::Quaterniond q_yaw = quaternionFromYaw(yaw);
+     149             :   msg->x                   = q_yaw.x();
+     150             :   msg->y                   = q_yaw.y();
+     151             :   msg->z                   = q_yaw.z();
+     152             :   msg->w                   = q_yaw.w();
+     153             : }
+     154             : 
+     155             : //}
+     156             : 
+     157             : /* setAngularVelocityMsgFromYawRate() //{ */
+     158             : 
+     159             : inline void setAngularVelocityMsgFromYawRate(double yaw_rate, geometry_msgs::Vector3* msg) {
+     160             :   assert(msg != NULL);
+     161             :   msg->x = 0.0;
+     162             :   msg->y = 0.0;
+     163             :   msg->z = yaw_rate;
+     164             : }
+     165             : 
+     166             : //}
+     167             : 
+     168             : /* getEulerAnglesFromQuaternion() //{ */
+     169             : 
+     170             : inline void getEulerAnglesFromQuaternion(const Eigen::Quaternion<double>& q, Eigen::Vector3d* euler_angles) {
+     171             :   {
+     172             :     assert(euler_angles != NULL);
+     173             : 
+     174             :     *euler_angles << std::atan2(2.0 * (q.w() * q.x() + q.y() * q.z()), 1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y())),
+     175             :         std::asin(2.0 * (q.w() * q.y() - q.z() * q.x())), std::atan2(2.0 * (q.w() * q.z() + q.x() * q.y()), 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z()));
+     176             :   }
+     177             : }
+     178             : 
+     179             : //}
+     180             : 
+     181             : /* skewMatrixFromVector() //{ */
+     182             : 
+     183             : inline void skewMatrixFromVector(const Eigen::Vector3d& vec, Eigen::Matrix3d* vec_skew) {
+     184             :   assert(vec_skew);
+     185             :   *vec_skew << 0.0f, -vec(2), vec(1), vec(2), 0.0f, -vec(0), -vec(1), vec(0), 0.0f;
+     186             : }
+     187             : 
+     188             : //}
+     189             : 
+     190             : /* vectorFromSkewMatrix() //{ */
+     191             : 
+     192             : inline bool vectorFromSkewMatrix(const Eigen::Matrix3d& vec_skew, Eigen::Vector3d* vec) {
+     193             :   assert(vec);
+     194             :   if ((vec_skew + vec_skew.transpose()).norm() < kSmallValueCheck) {
+     195             :     *vec << vec_skew(2, 1), vec_skew(0, 2), vec_skew(1, 0);
+     196             :     return true;
+     197             :   } else {
+     198             :     std::cerr << "[eth_mav_msgs] Matrix is not skew-symmetric." << std::endl;
+     199             :     *vec = Eigen::Vector3d::Zero();
+     200             :     return false;
+     201             :   }
+     202             : }
+     203             : 
+     204             : //}
+     205             : 
+     206             : /* isRotationMatrix() //{ */
+     207             : 
+     208             : inline bool isRotationMatrix(const Eigen::Matrix3d& mat) {
+     209             :   // Check that R^T * R = I
+     210             :   if ((mat.transpose() * mat - Eigen::Matrix3d::Identity()).norm() > kSmallValueCheck) {
+     211             :     std::cerr << "[eth_mav_msgs::common] Rotation matrix requirement violated (R^T * R = I)" << std::endl;
+     212             :     return false;
+     213             :   }
+     214             :   // Check that det(R) = 1
+     215             :   if (mat.determinant() - 1.0 > kSmallValueCheck) {
+     216             :     std::cerr << "[eth_mav_msgs::common] Rotation matrix requirement violated (det(R) = 1)" << std::endl;
+     217             :     return false;
+     218             :   }
+     219             :   return true;
+     220             : }
+     221             : 
+     222             : //}
+     223             : 
+     224             : /* matrixFromRotationVector() //{ */
+     225             : 
+     226             : // Rotation matrix from rotation vector as described in
+     227             : // "Computationally Efficient Trajectory Generation for Fully Actuated Multirotor Vehicles"
+     228             : // Brescianini 2018
+     229             : inline void matrixFromRotationVector(const Eigen::Vector3d& vec, Eigen::Matrix3d* mat) {
+     230             :   // R = (I + sin||r|| / ||r||) [r] + ((1 - cos||r||)/||r||^2) [r]^2
+     231             :   // where [r] is the skew matrix of r vector
+     232             :   assert(mat);
+     233             :   double          r_norm        = vec.norm();
+     234             :   Eigen::Matrix3d vec_skew_norm = Eigen::Matrix3d::Zero();
+     235             :   if (r_norm > 0.0) {
+     236             :     skewMatrixFromVector(vec / r_norm, &vec_skew_norm);
+     237             :   }
+     238             : 
+     239             :   *mat = Eigen::Matrix3d::Identity() + vec_skew_norm * std::sin(r_norm) + vec_skew_norm * vec_skew_norm * (1 - std::cos(r_norm));
+     240             : }
+     241             : 
+     242             : //}
+     243             : 
+     244             : /* vectorFromRotationMatrix() //{ */
+     245             : 
+     246             : // Rotation vector from rotation matrix as described in
+     247             : // "Computationally Efficient Trajectory Generation for Fully Actuated Multirotor Vehicles"
+     248             : // Brescianini 2018
+     249             : inline bool vectorFromRotationMatrix(const Eigen::Matrix3d& mat, Eigen::Vector3d* vec) {
+     250             :   // [r] = phi / 2sin(phi) * (R - R^T)
+     251             :   // where [r] is the skew matrix of r vector
+     252             :   // and phi satisfies 1 + 2cos(phi) = trace(R)
+     253             :   assert(vec);
+     254             : 
+     255             :   if (!isRotationMatrix(mat)) {
+     256             :     std::cerr << "[eth_mav_msgs::common] Not a rotation matrix." << std::endl;
+     257             :     return false;
+     258             :   }
+     259             : 
+     260             :   if ((mat - Eigen::Matrix3d::Identity()).norm() < kSmallValueCheck) {
+     261             :     *vec = Eigen::Vector3d::Zero();
+     262             :     return true;
+     263             :   }
+     264             : 
+     265             :   // Compute cosine of angle and clamp in range [-1, 1]
+     266             :   double cos_phi         = (mat.trace() - 1.0) / 2.0;
+     267             :   double cos_phi_clamped = boost::algorithm::clamp(cos_phi, -1.0, 1.0);
+     268             :   double phi             = std::acos(cos_phi_clamped);
+     269             : 
+     270             :   if (phi < kSmallValueCheck) {
+     271             :     *vec = Eigen::Vector3d::Zero();
+     272             :   } else {
+     273             :     Eigen::Matrix3d vec_skew = (mat - mat.transpose()) * phi / (2.0 * std::sin(phi));
+     274             :     Eigen::Vector3d vec_unskewed;
+     275             :     if (vectorFromSkewMatrix(vec_skew, &vec_unskewed)) {
+     276             :       *vec = vec_unskewed;
+     277             :     } else {
+     278             :       return false;
+     279             :     }
+     280             :   }
+     281             :   return true;
+     282             : }
+     283             : 
+     284             : //}
+     285             : 
+     286             : /* omegaFromRotationVector() //{ */
+     287             : 
+     288             : // Calculates angular velocity (omega) from rotation vector derivative
+     289             : // based on formula derived in "Finite rotations and angular velocity" by Asher
+     290             : // Peres
+     291             : inline Eigen::Vector3d omegaFromRotationVector(const Eigen::Vector3d& rot_vec, const Eigen::Vector3d& rot_vec_vel) {
+     292             :   double phi = rot_vec.norm();
+     293             :   if (std::abs(phi) < 1.0e-3) {
+     294             :     // This captures the case of zero rotation
+     295             :     return rot_vec_vel;
+     296             :   }
+     297             : 
+     298             :   double phi_inv   = 1.0 / phi;
+     299             :   double phi_2_inv = phi_inv / phi;
+     300             :   double phi_3_inv = phi_2_inv / phi;
+     301             : 
+     302             :   // Create skew-symmetric matrix from rotation vector
+     303             :   Eigen::Matrix3d phi_skew;
+     304             :   skewMatrixFromVector(rot_vec, &phi_skew);
+     305             : 
+     306             :   // Set up matrix to calculate omega
+     307             :   Eigen::Matrix3d W;
+     308             :   W = Eigen::MatrixXd::Identity(3, 3) + phi_skew * (1 - std::cos(phi)) * phi_2_inv + phi_skew * phi_skew * (phi - std::sin(phi)) * phi_3_inv;
+     309             :   return W * rot_vec_vel;
+     310             : }
+     311             : 
+     312             : //}
+     313             : 
+     314             : /* omegaDotFromRotationVector() //{ */
+     315             : 
+     316             : // Calculates angular acceleration (omegaDot) from rotation vector derivative
+     317             : // based on formula derived in "Finite rotations and angular velocity" by Asher
+     318             : // Peres
+     319             : inline Eigen::Vector3d omegaDotFromRotationVector(const Eigen::Vector3d& rot_vec, const Eigen::Vector3d& rot_vec_vel, const Eigen::Vector3d& rot_vec_acc) {
+     320             :   double phi = rot_vec.norm();
+     321             :   if (std::abs(phi) < 1.0e-3) {
+     322             :     // This captures the case of zero rotation
+     323             :     return rot_vec_acc;
+     324             :   }
+     325             : 
+     326             :   double phi_dot = rot_vec.dot(rot_vec_vel) / phi;
+     327             : 
+     328             :   double phi_inv   = 1.0 / phi;
+     329             :   double phi_2_inv = phi_inv / phi;
+     330             :   double phi_3_inv = phi_2_inv / phi;
+     331             :   double phi_4_inv = phi_3_inv / phi;
+     332             : 
+     333             : 
+     334             :   // Create skew-symmetric matrix from rotation vector and velocity
+     335             :   Eigen::Matrix3d phi_skew;
+     336             :   Eigen::Matrix3d phi_dot_skew;
+     337             : 
+     338             :   skewMatrixFromVector(rot_vec, &phi_skew);
+     339             :   skewMatrixFromVector(rot_vec_vel, &phi_dot_skew);
+     340             : 
+     341             :   // Set up matrices to calculate omega dot
+     342             :   Eigen::Matrix3d W_vel;
+     343             :   Eigen::Matrix3d W_acc;
+     344             :   W_vel = phi_skew * (phi * std::sin(phi) - 2.0f + 2.0f * std::cos(phi)) * phi_dot * phi_3_inv +
+     345             :           phi_skew * phi_skew * (-2.0f * phi - phi * std::cos(phi) + 3.0f * std::sin(phi)) * phi_dot * phi_4_inv +
+     346             :           phi_dot_skew * phi_skew * (phi - std::sin(phi)) * phi_3_inv;
+     347             : 
+     348             :   W_acc = Eigen::MatrixXd::Identity(3, 3) + phi_skew * (1.0f - std::cos(phi)) * phi_2_inv + phi_skew * phi_skew * (phi - std::sin(phi)) * phi_3_inv;
+     349             : 
+     350             :   return W_vel * rot_vec_vel + W_acc * rot_vec_acc;
+     351             : }
+     352             : 
+     353             : //}
+     354             : 
+     355             : /* getSquaredRotorSpeedsFromAllocationAndState() //{ */
+     356             : 
+     357             : // Calculate the nominal rotor rates given the MAV mass, allocation matrix,
+     358             : // angular velocity, angular acceleration, and body acceleration (normalized
+     359             : // thrust).
+     360             : //
+     361             : // [torques, thrust]' = A * n^2, where
+     362             : // torques = J * ang_acc + ang_vel x J
+     363             : // thrust = m * norm(acc)
+     364             : //
+     365             : // The allocation matrix A has of a hexacopter is:
+     366             : // A = K * B, where
+     367             : // K = diag(l*c_T, l*c_T, c_M, c_T),
+     368             : //     [ s  1  s -s -1 -s]
+     369             : // B = [-c  0  c  c  0 -c]
+     370             : //     [-1  1 -1  1 -1  1]
+     371             : //     [ 1  1  1  1  1  1],
+     372             : // l: arm length
+     373             : // c_T: thrust constant
+     374             : // c_M: moment constant
+     375             : // s: sin(30°)
+     376             : // c: cos(30°)
+     377             : //
+     378             : // The inverse can be computed computationally efficient:
+     379             : // A^-1 \approx B^pseudo * K^-1
+     380             : inline void getSquaredRotorSpeedsFromAllocationAndState(const Eigen::MatrixXd& allocation_inv, const Eigen::Vector3d& inertia, double mass,
+     381             :                                                         const Eigen::Vector3d& angular_velocity_B, const Eigen::Vector3d& angular_acceleration_B,
+     382             :                                                         const Eigen::Vector3d& acceleration_B, Eigen::VectorXd* rotor_rates_squared) {
+     383             :   const Eigen::Vector3d torque       = inertia.asDiagonal() * angular_acceleration_B + angular_velocity_B.cross(inertia.asDiagonal() * angular_velocity_B);
+     384             :   const double          thrust_force = mass * acceleration_B.norm();
+     385             :   Eigen::Vector4d       input;
+     386             :   input << torque, thrust_force;
+     387             :   *rotor_rates_squared = allocation_inv * input;
+     388             : }
+     389             : 
+     390             : //}
+     391             : 
+     392             : /* nanosecondsToSeconds() //{ */
+     393             : 
+     394             : inline double nanosecondsToSeconds(int64_t nanoseconds) {
+     395             :   double seconds = nanoseconds / kNumNanosecondsPerSecond;
+     396             :   return seconds;
+     397             : }
+     398             : 
+     399             : //}
+     400             : 
+     401             : /* secondsToNanoseconds() //{ */
+     402             : 
+     403             : inline int64_t secondsToNanoseconds(double seconds) {
+     404             :   int64_t nanoseconds = static_cast<int64_t>(seconds * kNumNanosecondsPerSecond);
+     405             :   return nanoseconds;
+     406             : }
+     407             : 
+     408             : //}
+     409             : 
+     410             : }  // namespace eth_mav_msgs
+     411             : 
+     412             : #endif  // eth_mav_msgs_COMMON_H
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.overview.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.overview.html new file mode 100644 index 0000000000..f59c0f0674 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.overview.html @@ -0,0 +1,123 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs/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 + overview + overview + overview + overview + overview + 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/include/eth_mav_msgs/common.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_mav_msgs/common.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..380603cff091ec67f270e981cf960298d7adee18 GIT binary patch literal 1579 zcmV+`2Gse9P)Kp2s@!4lVLUZye=h&5IK;y<1p9I5C53HMK!-|p05 z8Xphp44i*;z;%)|WO+lr02B$35l>>bNSgJNI75{Wh#pX1fYnrPq7e`AE0UT={@Gg0Z9pBap(4M1i5@ z%J&dQ^EZG;IG+W4Re>AJ$5@XO6@iMU@|i*?fCp8;2S5X3?!3bl-V&P6v~MRy>pf(z zp@t;YdyDZ>a-^L7Pr%rdqkr>AnYjJ4ay&mawN8T)ruusa z;I&kEQbcJ~h&n;F+W1xBIT&4jJHrnfFC>?FljBLE>_psN3|>r{60pG%+yRv9VW9jR z;Kf378LuYeu2E#fWNWGAG#uc45pX{96;=3pHFcmu-%3mC<~TF%i>Gz%WiO?yvH|p& z^ScFYAehJfO`Msl8O7VnYqXcD>M2vAKh}k5ahrR;0`CaCSR^WN&rPxPL1bT#H?pmC>U`4*5}@ZK zeU!wEt8hHrI}R&Uk%`~R?Zn`(&-~3kqX{N^&w!hl&+yr--5tdkI*=Wse{nb+4jm`U z!RHxW_kf<-ptJK#cr<;U%C)qzSgiKuQ@@89ae&+u&Zb?r+KQd-mxU?BZtKDrbR(s5 zn#cg=%L?!XW`;$KB{HU&<^5~Ua!N<`Vm)tU0fz?M^tnW=$J~r*T;k?4%a0Fw!WFvL zz@{U~$L27#0Cwa8E5(O7S)KfRi%EO)ZrwLaEVfA!vnLkm+F)aM6)yAM6&_(0a(;db zvc$*Y?o+^05e`5dV6aI%p;4@a4TdvoJGE$Zdc;@9ado{!BcEC)n=Ou}o|l77?}(Qo z@-~I)W4)Z||_5P&QYwM=D|82*96>J?_v8{pYdAmo&#KhB(GgM6)p9`Qtb~ zRzFs;#utf?^I|VfW5@ivcq>N{#-1mFL$is^!>Q-nqzi!3KjNs(%MZUH##r;3vFx6i zPHp*yonAn}#)Nh6>FiQy(@SwLVKi$^gQdoC!GxqTdb%vGvI;-=WK+pM3|N3AgMc<- zfYt4td^Ghcjg&SP25XxVz&VW4Vdp*tCLI4DW|>A2*X}9&+mZJC%1$2)N-_(`k{vO< z)A-;M0kAV&gYf~l!;S&>UsZ0H9#cTb7{}JmKldCVD}Yi4jRmv-I9Kx`;1oh>gyNm* zB3k1)KwXcHk~I;vtxp+R$BoH)4$x%3tjYGLQaJ>eTvWacKB^mTSQKO1a3$~vQs;1a zsDZm$us`u+L2W+VhwMPwtl4VM0#2F#p9NlJZD_@vAhGtOuIyD~ dx{ox(`3KrNS#m3rBFX>&002ovPDHLkV1fxT^?v{W literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func-sort-c.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func-sort-c.html new file mode 100644 index 0000000000..69450f1862 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func-sort-c.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgs - eigen_mav_msgs.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:202676.9 %
    Date:2024-04-09 22:03:33Functions:55100.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_mav_msgs::EigenTrajectoryPoint::getYaw() const849
    eth_mav_msgs::EigenTrajectoryPoint::setFromYawAcc(double)1408
    eth_mav_msgs::EigenTrajectoryPoint::setFromYawRate(double)1408
    eth_mav_msgs::EigenTrajectoryPoint::setFromYaw(double)2257
    eth_mav_msgs::EigenTrajectoryPoint::EigenTrajectoryPoint()2257
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func.html new file mode 100644 index 0000000000..8d72e40a9e --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.func.html @@ -0,0 +1,100 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgs - eigen_mav_msgs.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:202676.9 %
    Date:2024-04-09 22:03:33Functions:55100.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_mav_msgs::EigenTrajectoryPoint::setFromYaw(double)2257
    eth_mav_msgs::EigenTrajectoryPoint::setFromYawAcc(double)1408
    eth_mav_msgs::EigenTrajectoryPoint::setFromYawRate(double)1408
    eth_mav_msgs::EigenTrajectoryPoint::EigenTrajectoryPoint()2257
    eth_mav_msgs::EigenTrajectoryPoint::getYaw() const849
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.frameset.html new file mode 100644 index 0000000000..af395bc31c --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.html new file mode 100644 index 0000000000..dbfb4dc9f5 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.html @@ -0,0 +1,463 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgs - eigen_mav_msgs.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:202676.9 %
    Date:2024-04-09 22:03:33Functions:55100.0 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright 2015 Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  *     http://www.apache.org/licenses/LICENSE-2.0
+      13             : 
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #ifndef ETH_MAV_MSGS_EIGEN_MAV_MSGS_H
+      22             : #define ETH_MAV_MSGS_EIGEN_MAV_MSGS_H
+      23             : 
+      24             : #include <Eigen/Eigen>
+      25             : #include <deque>
+      26             : #include <iostream>
+      27             : 
+      28             : #include <eth_mav_msgs/common.h>
+      29             : 
+      30             : namespace eth_mav_msgs
+      31             : {
+      32             : 
+      33             : /// Actuated degrees of freedom.
+      34             : enum MavActuation
+      35             : {
+      36             :   DOF4 = 4,
+      37             :   DOF6 = 6
+      38             : };
+      39             : 
+      40             : /* struct EigenAttitudeThrust //{ */
+      41             : 
+      42             : struct EigenAttitudeThrust
+      43             : {
+      44             :   EigenAttitudeThrust() : attitude(Eigen::Quaterniond::Identity()), thrust(Eigen::Vector3d::Zero()) {
+      45             :   }
+      46             :   EigenAttitudeThrust(const Eigen::Quaterniond& _attitude, const Eigen::Vector3d& _thrust) {
+      47             :     attitude = _attitude;
+      48             :     thrust   = _thrust;
+      49             :   }
+      50             : 
+      51             :   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+      52             :   Eigen::Quaterniond attitude;
+      53             :   Eigen::Vector3d    thrust;
+      54             : };
+      55             : 
+      56             : //}
+      57             : 
+      58             : /* struct EigenAttitudeThrust //{ */
+      59             : 
+      60             : struct EigenActuators
+      61             : {
+      62             :   // TODO(ffurrer): Find a proper way of initializing :)
+      63             : 
+      64             :   EigenActuators(const Eigen::VectorXd& _angular_velocities) {
+      65             :     angular_velocities = _angular_velocities;
+      66             :   }
+      67             : 
+      68             :   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+      69             :   Eigen::VectorXd angles;              // In rad.
+      70             :   Eigen::VectorXd angular_velocities;  // In rad/s.
+      71             :   Eigen::VectorXd normalized;          // Everything else, normalized [-1 to 1].
+      72             : };
+      73             : 
+      74             : //}
+      75             : 
+      76             : /* struct EigenRateThrust //{ */
+      77             : 
+      78             : struct EigenRateThrust
+      79             : {
+      80             :   EigenRateThrust() : angular_rates(Eigen::Vector3d::Zero()), thrust(Eigen::Vector3d::Zero()) {
+      81             :   }
+      82             : 
+      83             :   EigenRateThrust(const Eigen::Vector3d& _angular_rates, const Eigen::Vector3d _thrust) : angular_rates(_angular_rates), thrust(_thrust) {
+      84             :   }
+      85             : 
+      86             :   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+      87             :   Eigen::Vector3d angular_rates;
+      88             :   Eigen::Vector3d thrust;
+      89             : };
+      90             : 
+      91             : //}
+      92             : 
+      93             : /* struct EigenTorqueThrust //{ */
+      94             : 
+      95             : struct EigenTorqueThrust
+      96             : {
+      97             :   EigenTorqueThrust() : torque(Eigen::Vector3d::Zero()), thrust(Eigen::Vector3d::Zero()) {
+      98             :   }
+      99             : 
+     100             :   EigenTorqueThrust(const Eigen::Vector3d& _torque, const Eigen::Vector3d _thrust) : torque(_torque), thrust(_thrust) {
+     101             :   }
+     102             : 
+     103             :   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+     104             :   Eigen::Vector3d torque;
+     105             :   Eigen::Vector3d thrust;
+     106             : };
+     107             : 
+     108             : //}
+     109             : 
+     110             : /* struct EigenRollPitchYawrateThrust //{ */
+     111             : 
+     112             : struct EigenRollPitchYawrateThrust
+     113             : {
+     114             :   EigenRollPitchYawrateThrust() : roll(0.0), pitch(0.0), yaw_rate(0.0), thrust(Eigen::Vector3d::Zero()) {
+     115             :   }
+     116             : 
+     117             :   EigenRollPitchYawrateThrust(double _roll, double _pitch, double _yaw_rate, const Eigen::Vector3d& _thrust)
+     118             :       : roll(_roll), pitch(_pitch), yaw_rate(_yaw_rate), thrust(_thrust) {
+     119             :   }
+     120             : 
+     121             :   double          roll;
+     122             :   double          pitch;
+     123             :   double          yaw_rate;
+     124             :   Eigen::Vector3d thrust;
+     125             : };
+     126             : 
+     127             : //}
+     128             : 
+     129             : /* class EigenMavState //{ */
+     130             : 
+     131             : /**
+     132             :  * \brief Container holding the state of a MAV: position, velocity, attitude and
+     133             :  * angular velocity.
+     134             :  *        In addition, holds the acceleration expressed in body coordinates,
+     135             :  * which is what the accelerometer
+     136             :  *        usually measures.
+     137             :  */
+     138             : class EigenMavState {
+     139             : public:
+     140             :   typedef std::vector<EigenMavState, Eigen::aligned_allocator<EigenMavState>> Vector;
+     141             :   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+     142             : 
+     143             :   /// Initializes all members to zero / identity.
+     144             :   EigenMavState()
+     145             :       : position_W(Eigen::Vector3d::Zero()),
+     146             :         velocity_W(Eigen::Vector3d::Zero()),
+     147             :         acceleration_B(Eigen::Vector3d::Zero()),
+     148             :         orientation_W_B(Eigen::Quaterniond::Identity()),
+     149             :         angular_velocity_B(Eigen::Vector3d::Zero()),
+     150             :         angular_acceleration_B(Eigen::Vector3d::Zero()) {
+     151             :   }
+     152             : 
+     153             :   EigenMavState(const Eigen::Vector3d& position_W, const Eigen::Vector3d& velocity_W, const Eigen::Vector3d& acceleration_B,
+     154             :                 const Eigen::Quaterniond& orientation_W_B, const Eigen::Vector3d& angular_velocity_B, const Eigen::Vector3d& angular_acceleration_B)
+     155             :       : position_W(position_W),
+     156             :         velocity_W(velocity_W),
+     157             :         acceleration_B(acceleration_B),
+     158             :         orientation_W_B(orientation_W_B),
+     159             :         angular_velocity_B(angular_velocity_B),
+     160             :         angular_acceleration_B(angular_acceleration_B) {
+     161             :   }
+     162             : 
+     163             :   std::string toString() const {
+     164             :     std::stringstream ss;
+     165             :     ss << "position:              " << position_W.transpose() << std::endl
+     166             :        << "velocity:              " << velocity_W.transpose() << std::endl
+     167             :        << "acceleration_body:     " << acceleration_B.transpose() << std::endl
+     168             :        << "orientation (w-x-y-z): " << orientation_W_B.w() << " " << orientation_W_B.x() << " " << orientation_W_B.y() << " " << orientation_W_B.z() << " "
+     169             :        << std::endl
+     170             :        << "angular_velocity_body: " << angular_velocity_B.transpose() << std::endl
+     171             :        << "angular_acceleration_body: " << angular_acceleration_B.transpose() << std::endl;
+     172             : 
+     173             :     return ss.str();
+     174             :   }
+     175             : 
+     176             :   Eigen::Vector3d    position_W;
+     177             :   Eigen::Vector3d    velocity_W;
+     178             :   Eigen::Vector3d    acceleration_B;
+     179             :   Eigen::Quaterniond orientation_W_B;
+     180             :   Eigen::Vector3d    angular_velocity_B;
+     181             :   Eigen::Vector3d    angular_acceleration_B;
+     182             : };
+     183             : 
+     184             : //}
+     185             : 
+     186             : /* struct EigenTrajectoryPoint //{ */
+     187             : 
+     188             : struct EigenTrajectoryPoint
+     189             : {
+     190             :   typedef std::vector<EigenTrajectoryPoint, Eigen::aligned_allocator<EigenTrajectoryPoint>> Vector;
+     191        2257 :   EigenTrajectoryPoint()
+     192        2257 :       : timestamp_ns(-1),
+     193             :         time_from_start_ns(0),
+     194           0 :         position_W(Eigen::Vector3d::Zero()),
+     195           0 :         velocity_W(Eigen::Vector3d::Zero()),
+     196           0 :         acceleration_W(Eigen::Vector3d::Zero()),
+     197           0 :         jerk_W(Eigen::Vector3d::Zero()),
+     198           0 :         snap_W(Eigen::Vector3d::Zero()),
+     199             :         orientation_W_B(Eigen::Quaterniond::Identity()),
+     200           0 :         angular_velocity_W(Eigen::Vector3d::Zero()),
+     201        4514 :         angular_acceleration_W(Eigen::Vector3d::Zero()),
+     202        2257 :         degrees_of_freedom(MavActuation::DOF4) {
+     203        2257 :   }
+     204             : 
+     205             :   EigenTrajectoryPoint(int64_t _time_from_start_ns, const Eigen::Vector3d& _position, const Eigen::Vector3d& _velocity, const Eigen::Vector3d& _acceleration,
+     206             :                        const Eigen::Vector3d& _jerk, const Eigen::Vector3d& _snap, const Eigen::Quaterniond& _orientation,
+     207             :                        const Eigen::Vector3d& _angular_velocity, const Eigen::Vector3d& _angular_acceleration,
+     208             :                        const MavActuation& _degrees_of_freedom = MavActuation::DOF4)
+     209             :       : time_from_start_ns(_time_from_start_ns),
+     210             :         position_W(_position),
+     211             :         velocity_W(_velocity),
+     212             :         acceleration_W(_acceleration),
+     213             :         jerk_W(_jerk),
+     214             :         snap_W(_snap),
+     215             :         orientation_W_B(_orientation),
+     216             :         angular_velocity_W(_angular_velocity),
+     217             :         angular_acceleration_W(_angular_acceleration),
+     218             :         degrees_of_freedom(_degrees_of_freedom) {
+     219             :   }
+     220             : 
+     221             :   EigenTrajectoryPoint(int64_t _time_from_start_ns, const Eigen::Vector3d& _position, const Eigen::Vector3d& _velocity, const Eigen::Vector3d& _acceleration,
+     222             :                        const Eigen::Vector3d& _jerk, const Eigen::Vector3d& _snap, const Eigen::Quaterniond& _orientation,
+     223             :                        const Eigen::Vector3d& _angular_velocity, const MavActuation& _degrees_of_freedom = MavActuation::DOF4)
+     224             :       : EigenTrajectoryPoint(_time_from_start_ns, _position, _velocity, _acceleration, _jerk, _snap, _orientation, _angular_velocity, Eigen::Vector3d::Zero(),
+     225             :                              _degrees_of_freedom) {
+     226             :   }
+     227             : 
+     228             :   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+     229             :   int64_t         timestamp_ns;  // Time since epoch, negative value = invalid timestamp.
+     230             :   int64_t         time_from_start_ns;
+     231             :   Eigen::Vector3d position_W;
+     232             :   Eigen::Vector3d velocity_W;
+     233             :   Eigen::Vector3d acceleration_W;
+     234             :   Eigen::Vector3d jerk_W;
+     235             :   Eigen::Vector3d snap_W;
+     236             : 
+     237             :   Eigen::Quaterniond orientation_W_B;
+     238             :   Eigen::Vector3d    angular_velocity_W;
+     239             :   Eigen::Vector3d    angular_acceleration_W;
+     240             :   MavActuation       degrees_of_freedom;
+     241             : 
+     242             :   // Accessors for making dealing with orientation/angular velocity easier.
+     243         849 :   inline double getYaw() const {
+     244         849 :     return yawFromQuaternion(orientation_W_B);
+     245             :   }
+     246             :   inline double getYawRate() const {
+     247             :     return angular_velocity_W.z();
+     248             :   }
+     249             :   inline double getYawAcc() const {
+     250             :     return angular_acceleration_W.z();
+     251             :   }
+     252             :   // WARNING: sets roll and pitch to 0.
+     253        2257 :   inline void setFromYaw(double yaw) {
+     254        2257 :     orientation_W_B = quaternionFromYaw(yaw);
+     255        2257 :   }
+     256        1408 :   inline void setFromYawRate(double yaw_rate) {
+     257        1408 :     angular_velocity_W.x() = 0.0;
+     258        1408 :     angular_velocity_W.y() = 0.0;
+     259        1408 :     angular_velocity_W.z() = yaw_rate;
+     260        1408 :   }
+     261        1408 :   inline void setFromYawAcc(double yaw_acc) {
+     262        1408 :     angular_acceleration_W.x() = 0.0;
+     263        1408 :     angular_acceleration_W.y() = 0.0;
+     264        1408 :     angular_acceleration_W.z() = yaw_acc;
+     265        1408 :   }
+     266             : 
+     267             :   std::string toString() const {
+     268             :     std::stringstream ss;
+     269             :     ss << "position:          " << position_W.transpose() << std::endl
+     270             :        << "velocity:          " << velocity_W.transpose() << std::endl
+     271             :        << "acceleration:      " << acceleration_W.transpose() << std::endl
+     272             :        << "jerk:              " << jerk_W.transpose() << std::endl
+     273             :        << "snap:              " << snap_W.transpose() << std::endl
+     274             :        << "yaw:               " << getYaw() << std::endl
+     275             :        << "yaw_rate:          " << getYawRate() << std::endl
+     276             :        << "yaw_acc:           " << getYawAcc() << std::endl;
+     277             : 
+     278             :     return ss.str();
+     279             :   }
+     280             : };
+     281             : 
+     282             : //}
+     283             : 
+     284             : /* EigenTrajectoryPoint operator* //{ */
+     285             : 
+     286             : // Operator overload to transform Trajectory Points according to the Eigen
+     287             : // interfaces (uses operator* for this).
+     288             : // Has to be outside of class.
+     289             : // Example:
+     290             : // Eigen::Affine3d transform; EigenTrajectoryPoint point;
+     291             : // EigenTrajectoryPoint transformed = transform * point;
+     292             : inline EigenTrajectoryPoint operator*(const Eigen::Affine3d& lhs, const EigenTrajectoryPoint& rhs) {
+     293             :   EigenTrajectoryPoint transformed(rhs);
+     294             :   transformed.position_W             = lhs * rhs.position_W;
+     295             :   transformed.velocity_W             = lhs.rotation() * rhs.velocity_W;
+     296             :   transformed.acceleration_W         = lhs.rotation() * rhs.acceleration_W;
+     297             :   transformed.jerk_W                 = lhs.rotation() * rhs.jerk_W;
+     298             :   transformed.snap_W                 = lhs.rotation() * rhs.snap_W;
+     299             :   transformed.orientation_W_B        = lhs.rotation() * rhs.orientation_W_B;
+     300             :   transformed.angular_velocity_W     = lhs.rotation() * rhs.angular_velocity_W;
+     301             :   transformed.angular_acceleration_W = lhs.rotation() * rhs.angular_acceleration_W;
+     302             :   return transformed;
+     303             : }
+     304             : 
+     305             : //}
+     306             : 
+     307             : /* struct EigenOdometry //{ */
+     308             : 
+     309             : struct EigenOdometry
+     310             : {
+     311             :   EigenOdometry()
+     312             :       : timestamp_ns(-1),
+     313             :         position_W(Eigen::Vector3d::Zero()),
+     314             :         orientation_W_B(Eigen::Quaterniond::Identity()),
+     315             :         velocity_B(Eigen::Vector3d::Zero()),
+     316             :         angular_velocity_B(Eigen::Vector3d::Zero()) {
+     317             :   }
+     318             : 
+     319             :   EigenOdometry(const Eigen::Vector3d& _position, const Eigen::Quaterniond& _orientation, const Eigen::Vector3d& _velocity_body,
+     320             :                 const Eigen::Vector3d& _angular_velocity)
+     321             :       : position_W(_position), orientation_W_B(_orientation), velocity_B(_velocity_body), angular_velocity_B(_angular_velocity) {
+     322             :   }
+     323             : 
+     324             :   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+     325             :   int64_t                     timestamp_ns;  // Time since epoch, negative value = invalid timestamp.
+     326             :   Eigen::Vector3d             position_W;
+     327             :   Eigen::Quaterniond          orientation_W_B;
+     328             :   Eigen::Vector3d             velocity_B;  // Velocity in expressed in the Body frame!
+     329             :   Eigen::Vector3d             angular_velocity_B;
+     330             :   Eigen::Matrix<double, 6, 6> pose_covariance_;
+     331             :   Eigen::Matrix<double, 6, 6> twist_covariance_;
+     332             : 
+     333             :   // Accessors for making dealing with orientation/angular velocity easier.
+     334             :   inline double getYaw() const {
+     335             :     return yawFromQuaternion(orientation_W_B);
+     336             :   }
+     337             :   inline void getEulerAngles(Eigen::Vector3d* euler_angles) const {
+     338             :     getEulerAnglesFromQuaternion(orientation_W_B, euler_angles);
+     339             :   }
+     340             :   inline double getYawRate() const {
+     341             :     return angular_velocity_B.z();
+     342             :   }
+     343             :   // WARNING: sets roll and pitch to 0.
+     344             :   inline void setFromYaw(double yaw) {
+     345             :     orientation_W_B = quaternionFromYaw(yaw);
+     346             :   }
+     347             :   inline void setFromYawRate(double yaw_rate) {
+     348             :     angular_velocity_B.x() = 0.0;
+     349             :     angular_velocity_B.y() = 0.0;
+     350             :     angular_velocity_B.z() = yaw_rate;
+     351             :   }
+     352             : 
+     353             :   inline Eigen::Vector3d getVelocityWorld() const {
+     354             :     return orientation_W_B * velocity_B;
+     355             :   }
+     356             :   inline void setVelocityWorld(const Eigen::Vector3d& velocity_world) {
+     357             :     velocity_B = orientation_W_B.inverse() * velocity_world;
+     358             :   }
+     359             : };
+     360             : 
+     361             : //}
+     362             : 
+     363             : // TODO(helenol): replaced with aligned allocator headers from Simon.
+     364             : #define MAV_MSGS_CONCATENATE(x, y) x##y
+     365             : #define MAV_MSGS_CONCATENATE2(x, y) MAV_MSGS_CONCATENATE(x, y)
+     366             : #define MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EIGEN_TYPE)                                                               \
+     367             :   typedef std::vector<EIGEN_TYPE, Eigen::aligned_allocator<EIGEN_TYPE>> MAV_MSGS_CONCATENATE2(EIGEN_TYPE, Vector); \
+     368             :   typedef std::deque<EIGEN_TYPE, Eigen::aligned_allocator<EIGEN_TYPE>>  MAV_MSGS_CONCATENATE2(EIGEN_TYPE, Deque);
+     369             : 
+     370             : MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenAttitudeThrust)
+     371             : MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenActuators)
+     372             : MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenRateThrust)
+     373             : MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenTrajectoryPoint)
+     374             : MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenRollPitchYawrateThrust)
+     375             : MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenOdometry)
+     376             : 
+     377             : }  // namespace eth_mav_msgs
+     378             : 
+     379             : #endif  // MAV_MSGS_EIGEN_MAV_MSGS_H
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.overview.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.overview.html new file mode 100644 index 0000000000..cf24046890 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.overview.html @@ -0,0 +1,115 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.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 + + +
    + Top

    + Overview +
    + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_mav_msgs/eigen_mav_msgs.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..93ee2691c30fff5c887d753cc2cb3de1f8f13009 GIT binary patch literal 1499 zcmV<11tj{3P)LXMShU+pK3{|pEa0+ip<790TSH6UTy z|L;GL;JC979eDigS9JY))q#AeAsf~KIi{O6P4+ovhDATXcEIumWTt!*g?bXb zV=C@(zUr*a*}`=_7=vAOr0OtmhIQ7%VBr!lPVqK12^j-|Mk!*1*zKm?ivIz+{AtcZ z5#lnwNXDFZ2ZnHA*3m%=ID5Ad%BCnv=kb6udSn5cgAS-gtBfm$-m!m zv4DOGa`qFnFGS%)3hm*U4d?k(Qic$4kD zSF&c|y&ztx5wP5QxJ>}%k{SV+I*F<~IagUCV}1`L_VPUf9Ju%oTM9rHyQNhVmXW($ z+|t86hvi#T`ith(h~mMYnpMC#cse^=rG>Fm1S}Af@JW$qc&wwd=0$>IzYWq%Z8X!` zk$Ze=Zv{p6SX0B1x`=3KYJ#PbVOrd;0}ZGKRKY=*NwKm1!WiBp%_3zM{bAvk%_Fjq zkd_H$3sXQJ+$k^$G_i(dnoiy#%am$$ggn5bSdpvW{1IgYz$$dk0<-$CjwuofG6g^h z1muqe7|>i*EE%qw*j#0E|DnLCp+R|Sf z%WH)jlB6UzXph)btu{?lhWy~8NPKh@i3Fk>p&|jePc2YbuJBSN6L4AdOp4^+gsSDaGDQwpCST0 z?l@hF49wR8*wUK>Vg>9m=44c;Y>^PK?$}I@q$JSV2zRfqiQ!RPQn9nm()(w7CE$`V zcv0}tLe4Td^JB(VVW$#4Y*TC?NEV9nCA0eR$SR&yXV_a z!f^+{>BIP`{Ln?>{TJ&9bYMWm20CrDkysR2kLA2m{Mi8 zZst&}?dup@qX^YjcKpeV3e?$4?Skj*I`EwZ9xX{_aW+m}`_pm-H&++7y&;Zoa*cIt zB~-|8sU-b$H5@ES8}*zE_ZO6_8H@?J68``I002ovPDHLkV1gb2 B%+CM- literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail-sort-f.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail-sort-f.html new file mode 100644 index 0000000000..91eaf04dd1 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgsHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:243080.0 %
    Date:2024-04-09 22:03:33Functions:77100.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 +
    100.0%
    +     		100.0 %4 / 4100.0 %2 / 2
    <unnamed>100.0 %4 / 4100.0 %2 / 2
    eigen_mav_msgs.h +
    76.9%76.9%
    +     		76.9 %20 / 26100.0 %5 / 5
    <unnamed>76.9 %20 / 26100.0 %5 / 5
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail-sort-l.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail-sort-l.html new file mode 100644 index 0000000000..4b125f04bd --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgsHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:243080.0 %
    Date:2024-04-09 22:03:33Functions:77100.0 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
    eigen_mav_msgs.h +
    76.9%76.9%
    +     		76.9 %20 / 26100.0 %5 / 5
    <unnamed>76.9 %20 / 26100.0 %5 / 5
    common.h +
    100.0%
    +     		100.0 %4 / 4100.0 %2 / 2
    <unnamed>100.0 %4 / 4100.0 %2 / 2
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail.html new file mode 100644 index 0000000000..5a0f8a57a8 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgsHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:243080.0 %
    Date:2024-04-09 22:03:33Functions:77100.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 +
    100.0%
    +     		100.0 %4 / 4100.0 %2 / 2
    <unnamed>100.0 %4 / 4100.0 %2 / 2
    eigen_mav_msgs.h +
    76.9%76.9%
    +     		76.9 %20 / 26100.0 %5 / 5
    <unnamed>76.9 %20 / 26100.0 %5 / 5
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-sort-f.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-sort-f.html new file mode 100644 index 0000000000..d20a987f8a --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgsHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:243080.0 %
    Date:2024-04-09 22:03:33Functions:77100.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 +
    100.0%
    +     		100.0 %4 / 4100.0 %2 / 2
    eigen_mav_msgs.h +
    76.9%76.9%
    +     		76.9 %20 / 26100.0 %5 / 5
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-sort-l.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-sort-l.html new file mode 100644 index 0000000000..0427d4d6d5 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgsHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:243080.0 %
    Date:2024-04-09 22:03:33Functions:77100.0 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
    eigen_mav_msgs.h +
    76.9%76.9%
    +     		76.9 %20 / 26100.0 %5 / 5
    common.h +
    100.0%
    +     		100.0 %4 / 4100.0 %2 / 2
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_mav_msgs/index.html b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index.html new file mode 100644 index 0000000000..22cbf9aaf4 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_mav_msgs/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_mav_msgs + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_mav_msgsHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:243080.0 %
    Date:2024-04-09 22:03:33Functions:77100.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 +
    100.0%
    +     		100.0 %4 / 4100.0 %2 / 2
    eigen_mav_msgs.h +
    76.9%76.9%
    +     		76.9 %20 / 26100.0 %5 / 5
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func-sort-c.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func-sort-c.html new file mode 100644 index 0000000000..6d5f31f03b --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func-sort-c.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - extremum.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:88100.0 %
    Date:2024-04-09 22:03:33Functions:44100.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::Extremum::operator>(eth_trajectory_generation::Extremum const&) const2970
    eth_trajectory_generation::Extremum::Extremum(double, double, int)13366
    eth_trajectory_generation::Extremum::Extremum()22564
    eth_trajectory_generation::Extremum::operator<(eth_trajectory_generation::Extremum const&) const29702
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func.html new file mode 100644 index 0000000000..616280f5f4 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.func.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - extremum.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:88100.0 %
    Date:2024-04-09 22:03:33Functions:44100.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::Extremum::Extremum(double, double, int)13366
    eth_trajectory_generation::Extremum::Extremum()22564
    eth_trajectory_generation::Extremum::operator>(eth_trajectory_generation::Extremum const&) const2970
    eth_trajectory_generation::Extremum::operator<(eth_trajectory_generation::Extremum const&) const29702
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.frameset.html new file mode 100644 index 0000000000..83207571c1 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.html new file mode 100644 index 0000000000..ed06b3dafe --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.html @@ -0,0 +1,143 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - extremum.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:88100.0 %
    Date:2024-04-09 22:03:33Functions:44100.0 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #ifndef ETH_TRAJECTORY_GENERATION_EXTREMUM_H_
+      22             : #define ETH_TRAJECTORY_GENERATION_EXTREMUM_H_
+      23             : 
+      24             : #include <iostream>
+      25             : 
+      26             : namespace eth_trajectory_generation
+      27             : {
+      28             : 
+      29             : // Container holding the properties of an extremum (time, value,
+      30             : // segment where it occurred).
+      31             : struct Extremum
+      32             : {
+      33             : public:
+      34       22564 :   Extremum() : time(0.0), value(0.0), segment_idx(0) {
+      35       22564 :   }
+      36             : 
+      37       13366 :   Extremum(double _time, double _value, int _segment_idx) : time(_time), value(_value), segment_idx(_segment_idx) {
+      38       13366 :   }
+      39             : 
+      40       29702 :   bool operator<(const Extremum& rhs) const {
+      41       29702 :     return value < rhs.value;
+      42             :   }
+      43        2970 :   bool operator>(const Extremum& rhs) const {
+      44        2970 :     return value > rhs.value;
+      45             :   }
+      46             : 
+      47             :   double time;         // Time where the extremum occurs, relative to the segment start.
+      48             :   double value;        // Value of the extremum at time.
+      49             :   int    segment_idx;  // Index of the segment where the extremum occurs.
+      50             : };
+      51             : 
+      52             : inline std::ostream& operator<<(std::ostream& stream, const Extremum& e) {
+      53             :   stream << "time: " << e.time << ", value: " << e.value << ", segment idx: " << e.segment_idx << std::endl;
+      54             :   return stream;
+      55             : }
+      56             : 
+      57             : }  // namespace eth_trajectory_generation
+      58             : 
+      59             : #endif  // ETH_TRAJECTORY_GENERATION_EXTREMUM_H_
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.overview.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.overview.html new file mode 100644 index 0000000000..3942a2ca95 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.overview.html @@ -0,0 +1,35 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h + + + + + + + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + overview + + +
    + Top

    + Overview +
    + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/extremum.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..89983bd960d0a4c91fe9a3a720c166c80b032044 GIT binary patch literal 437 zcmV;m0ZRUfP)&5s6IF&V&PPnkuC7Jqw>~hcuN1!6_)Y!VzQ!!o)9pH>d zQx85D&`?r=p-BKDA?lOH}32I`b5@RY;$@lP`g9cfK_*r%!?p181nI z-yfV@JYpog$7KxdSjCH?9`~+x2ox``8;^Ql3vR>73`@Oi+153Wacg7xZP{B`;xc4m z43>c;%7m*;v9d?bIj;YqUbn`OhL|~f9t}c)Zafah_&e%xvx3ZTf_<~n)?lsDS@i_M zadIyAgt5+tJ@Dwc;3lG+$)$B-whQG2Y2iAuTo}7+1E@EI#S3`WpF@4V_dx1jm}eSu f$t4_=p`CFrI(2jF5TU}700000NkvXXu0mjfJC(c= literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail-sort-f.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail-sort-f.html new file mode 100644 index 0000000000..0f389b547c --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail-sort-f.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/implHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:31664349.1 %
    Date:2024-04-09 22:03:33Functions:193357.6 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
    polynomial_optimization_nonlinear_impl.h +
    34.3%34.3%
    +     		34.3 %147 / 42942.1 %8 / 19
    <unnamed>34.3 %147 / 42942.1 %8 / 19
    polynomial_optimization_linear_impl.h +
    79.0%79.0%
    +     		79.0 %169 / 21478.6 %11 / 14
    <unnamed>79.0 %169 / 21478.6 %11 / 14
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail-sort-l.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail-sort-l.html new file mode 100644 index 0000000000..86b0918580 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail-sort-l.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/implHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:31664349.1 %
    Date:2024-04-09 22:03:33Functions:193357.6 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
    polynomial_optimization_nonlinear_impl.h +
    34.3%34.3%
    +     		34.3 %147 / 42942.1 %8 / 19
    <unnamed>34.3 %147 / 42942.1 %8 / 19
    polynomial_optimization_linear_impl.h +
    79.0%79.0%
    +     		79.0 %169 / 21478.6 %11 / 14
    <unnamed>79.0 %169 / 21478.6 %11 / 14
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail.html new file mode 100644 index 0000000000..5c689029aa --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-detail.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/implHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:31664349.1 %
    Date:2024-04-09 22:03:33Functions:193357.6 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
    polynomial_optimization_linear_impl.h +
    79.0%79.0%
    +     		79.0 %169 / 21478.6 %11 / 14
    <unnamed>79.0 %169 / 21478.6 %11 / 14
    polynomial_optimization_nonlinear_impl.h +
    34.3%34.3%
    +     		34.3 %147 / 42942.1 %8 / 19
    <unnamed>34.3 %147 / 42942.1 %8 / 19
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-sort-f.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-sort-f.html new file mode 100644 index 0000000000..b75b203190 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-sort-f.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/implHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:31664349.1 %
    Date:2024-04-09 22:03:33Functions:193357.6 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
    polynomial_optimization_nonlinear_impl.h +
    34.3%34.3%
    +     		34.3 %147 / 42942.1 %8 / 19
    polynomial_optimization_linear_impl.h +
    79.0%79.0%
    +     		79.0 %169 / 21478.6 %11 / 14
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-sort-l.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-sort-l.html new file mode 100644 index 0000000000..cfef8cffac --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index-sort-l.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/implHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:31664349.1 %
    Date:2024-04-09 22:03:33Functions:193357.6 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
    polynomial_optimization_nonlinear_impl.h +
    34.3%34.3%
    +     		34.3 %147 / 42942.1 %8 / 19
    polynomial_optimization_linear_impl.h +
    79.0%79.0%
    +     		79.0 %169 / 21478.6 %11 / 14
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index.html new file mode 100644 index 0000000000..1b67fd6845 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/index.html @@ -0,0 +1,112 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/implHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:31664349.1 %
    Date:2024-04-09 22:03:33Functions:193357.6 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
    polynomial_optimization_linear_impl.h +
    79.0%79.0%
    +     		79.0 %169 / 21478.6 %11 / 14
    polynomial_optimization_nonlinear_impl.h +
    34.3%34.3%
    +     		34.3 %147 / 42942.1 %8 / 19
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.func-sort-c.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.func-sort-c.html new file mode 100644 index 0000000000..775110826b --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.func-sort-c.html @@ -0,0 +1,136 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl - polynomial_optimization_linear_impl.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:16921479.0 %
    Date:2024-04-09 22:03:33Functions:111478.6 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::PolynomialOptimization<10>::setFreeConstraints(std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, std::allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > > const&)0
    eth_trajectory_generation::PolynomialOptimization<10>::computeSegmentMaximumMagnitudeCandidates(int, eth_trajectory_generation::Segment const&, double, double, std::vector<double, std::allocator<double> >*)0
    eth_trajectory_generation::PolynomialOptimization<10>::computeMaximumOfMagnitude(int, std::vector<eth_trajectory_generation::Extremum, std::allocator<eth_trajectory_generation::Extremum> >*) const0
    eth_trajectory_generation::PolynomialOptimization<10>::setupFromVertices(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&, std::vector<double, std::allocator<double> > const&, int)16
    eth_trajectory_generation::PolynomialOptimization<10>::setupConstraintReorderingMatrix()16
    eth_trajectory_generation::PolynomialOptimization<10>::PolynomialOptimization(unsigned long)16
    eth_trajectory_generation::PolynomialOptimization<10>::computeCost() const1706
    eth_trajectory_generation::PolynomialOptimization<10>::solveLinear()1933
    eth_trajectory_generation::PolynomialOptimization<10>::updateSegmentsFromCompactConstraints()1933
    eth_trajectory_generation::PolynomialOptimization<10>::constructR(Eigen::SparseMatrix<double, 0, int>*) const1933
    eth_trajectory_generation::PolynomialOptimization<10>::updateSegmentTimes(std::vector<double, std::allocator<double> > const&)1949
    eth_trajectory_generation::PolynomialOptimization<10>::setupMappingMatrix(double, Eigen::Matrix<double, 10, 10, 0, 10, 10>*)18901
    eth_trajectory_generation::PolynomialOptimization<10>::invertMappingMatrix(Eigen::Matrix<double, 10, 10, 0, 10, 10> const&, Eigen::Matrix<double, 10, 10, 0, 10, 10>*)18901
    eth_trajectory_generation::PolynomialOptimization<10>::computeQuadraticCostJacobian(int, double, Eigen::Matrix<double, 10, 10, 0, 10, 10>*)18901
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.func.html new file mode 100644 index 0000000000..60366b2f81 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.func.html @@ -0,0 +1,136 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl - polynomial_optimization_linear_impl.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:16921479.0 %
    Date:2024-04-09 22:03:33Functions:111478.6 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::PolynomialOptimization<10>::solveLinear()1933
    eth_trajectory_generation::PolynomialOptimization<10>::setupFromVertices(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&, std::vector<double, std::allocator<double> > const&, int)16
    eth_trajectory_generation::PolynomialOptimization<10>::setFreeConstraints(std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, std::allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > > const&)0
    eth_trajectory_generation::PolynomialOptimization<10>::setupMappingMatrix(double, Eigen::Matrix<double, 10, 10, 0, 10, 10>*)18901
    eth_trajectory_generation::PolynomialOptimization<10>::updateSegmentTimes(std::vector<double, std::allocator<double> > const&)1949
    eth_trajectory_generation::PolynomialOptimization<10>::invertMappingMatrix(Eigen::Matrix<double, 10, 10, 0, 10, 10> const&, Eigen::Matrix<double, 10, 10, 0, 10, 10>*)18901
    eth_trajectory_generation::PolynomialOptimization<10>::computeQuadraticCostJacobian(int, double, Eigen::Matrix<double, 10, 10, 0, 10, 10>*)18901
    eth_trajectory_generation::PolynomialOptimization<10>::setupConstraintReorderingMatrix()16
    eth_trajectory_generation::PolynomialOptimization<10>::updateSegmentsFromCompactConstraints()1933
    eth_trajectory_generation::PolynomialOptimization<10>::computeSegmentMaximumMagnitudeCandidates(int, eth_trajectory_generation::Segment const&, double, double, std::vector<double, std::allocator<double> >*)0
    eth_trajectory_generation::PolynomialOptimization<10>::PolynomialOptimization(unsigned long)16
    eth_trajectory_generation::PolynomialOptimization<10>::constructR(Eigen::SparseMatrix<double, 0, int>*) const1933
    eth_trajectory_generation::PolynomialOptimization<10>::computeCost() const1706
    eth_trajectory_generation::PolynomialOptimization<10>::computeMaximumOfMagnitude(int, std::vector<eth_trajectory_generation::Extremum, std::allocator<eth_trajectory_generation::Extremum> >*) const0
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.frameset.html new file mode 100644 index 0000000000..545a529e28 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.html new file mode 100644 index 0000000000..aca6295a4f --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.html @@ -0,0 +1,708 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl - polynomial_optimization_linear_impl.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:16921479.0 %
    Date:2024-04-09 22:03:33Functions:111478.6 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #ifndef eth_trajectory_generation_IMPL_POLYNOMIAL_OPTIMIZATION_LINEAR_IMPL_H_
+      22             : #define eth_trajectory_generation_IMPL_POLYNOMIAL_OPTIMIZATION_LINEAR_IMPL_H_
+      23             : 
+      24             : #include <eth_trajectory_generation/misc.h>
+      25             : #include <Eigen/Sparse>
+      26             : #include <set>
+      27             : #include <tuple>
+      28             : 
+      29             : // fixes error due to std::iota (has been introduced in c++ standard lately
+      30             : // and may cause compilation errors depending on compiler)
+      31             : #if __cplusplus <= 199711L
+      32             : #include <algorithm>
+      33             : #else
+      34             : #include <numeric>
+      35             : #endif
+      36             : 
+      37             : #include <eth_trajectory_generation/convolution.h>
+      38             : 
+      39             : namespace eth_trajectory_generation
+      40             : {
+      41             : 
+      42             : /* PolynomialOptimization() //{ */
+      43             : 
+      44             : template <int _N>
+      45          16 : PolynomialOptimization<_N>::PolynomialOptimization(size_t dimension)
+      46             :     : dimension_(dimension),
+      47             :       derivative_to_optimize_(derivative_order::INVALID),
+      48             :       n_vertices_(0),
+      49             :       n_segments_(0),
+      50             :       n_all_constraints_(0),
+      51             :       n_fixed_constraints_(0),
+      52          16 :       n_free_constraints_(0) {
+      53          16 :   fixed_constraints_compact_.resize(dimension_);
+      54          16 :   free_constraints_compact_.resize(dimension_);
+      55          16 : }
+      56             : 
+      57             : //}
+      58             : 
+      59             : /* setupFromVertices() //{ */
+      60             : 
+      61             : template <int _N>
+      62          16 : bool PolynomialOptimization<_N>::setupFromVertices(const Vertex::Vector& vertices, const std::vector<double>& times, int derivative_to_optimize) {
+      63          16 :   CHECK(derivative_to_optimize >= 0 && derivative_to_optimize <= kHighestDerivativeToOptimize)
+      64           0 :       << "You tried to optimize the " << derivative_to_optimize << "th derivative of position on a " << N
+      65           0 :       << "th order polynomial. This is not possible, you either need a higher "
+      66             :          "order polynomial or a smaller derivative to optimize.";
+      67             : 
+      68          16 :   derivative_to_optimize_ = derivative_to_optimize;
+      69          16 :   vertices_               = vertices;
+      70          16 :   segment_times_          = times;
+      71             : 
+      72          16 :   n_vertices_ = vertices.size();
+      73          16 :   n_segments_ = n_vertices_ - 1;
+      74             : 
+      75          16 :   segments_.resize(n_segments_, Segment(N, dimension_));
+      76             : 
+      77          16 :   CHECK(n_vertices_ == times.size() + 1) << "Size of times must be one less than positions.";
+      78             : 
+      79          16 :   inverse_mapping_matrices_.resize(n_segments_);
+      80          16 :   cost_matrices_.resize(n_segments_);
+      81             : 
+      82             :   // Iterate through all vertices and remove invalid constraints (order too
+      83             :   // high).
+      84         197 :   for (size_t vertex_idx = 0; vertex_idx < n_vertices_; ++vertex_idx) {
+      85         181 :     Vertex& vertex = vertices_[vertex_idx];
+      86             : 
+      87             :     // Check if we have valid constraints.
+      88         181 :     bool   vertex_valid = true;
+      89         362 :     Vertex vertex_tmp(dimension_);
+      90         438 :     for (Vertex::Constraints::const_iterator it = vertex.cBegin(); it != vertex.cEnd(); ++it) {
+      91         257 :       if (it->first > kHighestDerivativeToOptimize) {
+      92           0 :         vertex_valid = false;
+      93           0 :         LOG(WARNING) << "Invalid constraint on vertex " << vertex_idx << ": maximum possible derivative is " << kHighestDerivativeToOptimize
+      94           0 :                      << ", but was set to " << it->first << ". Ignoring constraint";
+      95             :       } else {
+      96         257 :         vertex_tmp.addConstraint(it->first, it->second);
+      97             :       }
+      98             :     }
+      99         181 :     if (!vertex_valid) {
+     100           0 :       vertex = vertex_tmp;
+     101             :     }
+     102             :   }
+     103          16 :   updateSegmentTimes(times);
+     104          16 :   setupConstraintReorderingMatrix();
+     105          16 :   return true;
+     106             : }
+     107             : 
+     108             : //}
+     109             : 
+     110             : /* setupMappingMatrix() //{ */
+     111             : 
+     112             : template <int _N>
+     113       18901 : void PolynomialOptimization<_N>::setupMappingMatrix(double segment_time, SquareMatrix* A) {
+     114             :   // The sum of fixed/free variables has to be equal on both ends of the
+     115             :   // segment.
+     116             :   // Thus, A is created as [A(t=0); A(t=segment_time)].
+     117      113406 :   for (int i = 0; i < N / 2; ++i) {
+     118       94505 :     A->row(i)         = Polynomial::baseCoeffsWithTime(N, i, 0.0);
+     119       94505 :     A->row(i + N / 2) = Polynomial::baseCoeffsWithTime(N, i, segment_time);
+     120             :   }
+     121       18901 : }
+     122             : 
+     123             : //}
+     124             : 
+     125             : /* computeCost() //{ */
+     126             : 
+     127             : template <int _N>
+     128        1706 : double PolynomialOptimization<_N>::computeCost() const {
+     129        1706 :   CHECK(n_segments_ == segments_.size() && n_segments_ == cost_matrices_.size());
+     130        1706 :   double cost = 0;
+     131       18782 :   for (size_t segment_idx = 0; segment_idx < n_segments_; ++segment_idx) {
+     132       17076 :     const SquareMatrix& Q       = cost_matrices_[segment_idx];
+     133       17076 :     const Segment&      segment = segments_[segment_idx];
+     134       85380 :     for (size_t dimension_idx = 0; dimension_idx < dimension_; ++dimension_idx) {
+     135       68304 :       const Eigen::VectorXd c            = segment[dimension_idx].getCoefficients(derivative_order::POSITION);
+     136       68304 :       const double          partial_cost = c.transpose() * Q * c;
+     137       68304 :       cost += partial_cost;
+     138             :     }
+     139             :   }
+     140        1706 :   return 0.5 * cost;  // cost = 0.5 * c^T * Q * c
+     141             : }
+     142             : 
+     143             : //}
+     144             : 
+     145             : /* invertMappingMatrix() //{ */
+     146             : 
+     147             : template <int _N>
+     148       18901 : void PolynomialOptimization<_N>::invertMappingMatrix(const SquareMatrix& mapping_matrix, SquareMatrix* inverse_mapping_matrix) {
+     149             :   // The mapping matrix has the following structure:
+     150             :   // [ x 0 0 0 0 0 ]
+     151             :   // [ 0 x 0 0 0 0 ]
+     152             :   // [ 0 0 x 0 0 0 ]
+     153             :   // [ x x x x x x ]
+     154             :   // [ 0 x x x x x ]
+     155             :   // [ 0 0 x x x x ]
+     156             :   // ==>
+     157             :   // [ A_diag B=0 ]
+     158             :   // [ C      D   ]
+     159             :   // We make use of the Schur-complement, so the inverse is:
+     160             :   // [ inv(A_diag)               0      ]
+     161             :   // [ -inv(D) * C * inv(A_diag) inv(D) ]
+     162       18901 :   const int half_n = N / 2;
+     163             : 
+     164             :   // "template" keyword required below as half_n is dependent on the template
+     165             :   // parameter.
+     166       18901 :   const Eigen::Matrix<double, half_n, 1>      A_diag = mapping_matrix.template block<half_n, half_n>(0, 0).diagonal();
+     167       18901 :   const Eigen::Matrix<double, half_n, half_n> A_inv  = A_diag.cwiseInverse().asDiagonal();
+     168             : 
+     169       18901 :   const Eigen::Matrix<double, half_n, half_n> C = mapping_matrix.template block<half_n, half_n>(half_n, 0);
+     170             : 
+     171       18901 :   const Eigen::Matrix<double, half_n, half_n> D_inv = mapping_matrix.template block<half_n, half_n>(half_n, half_n).inverse();
+     172             : 
+     173       18901 :   inverse_mapping_matrix->template block<half_n, half_n>(0, 0) = A_inv;
+     174       18901 :   inverse_mapping_matrix->template block<half_n, half_n>(0, half_n).setZero();
+     175       18901 :   inverse_mapping_matrix->template block<half_n, half_n>(half_n, 0)      = -D_inv * C * A_inv;
+     176       18901 :   inverse_mapping_matrix->template block<half_n, half_n>(half_n, half_n) = D_inv;
+     177       18901 : }
+     178             : 
+     179             : //}
+     180             : 
+     181             : /* setupConstraintReorderingMatrix() //{ */
+     182             : 
+     183             : template <int _N>
+     184          16 : void PolynomialOptimization<_N>::setupConstraintReorderingMatrix() {
+     185             :   typedef Eigen::Triplet<double> Triplet;
+     186          32 :   std::vector<Triplet>           reordering_list;
+     187             : 
+     188          16 :   const size_t n_vertices = vertices_.size();
+     189             : 
+     190          32 :   std::vector<Constraint> all_constraints;
+     191          32 :   std::set<Constraint>    fixed_constraints;
+     192          16 :   std::set<Constraint>    free_constraints;
+     193             : 
+     194          16 :   all_constraints.reserve(n_vertices_ * N / 2);  // Will have exactly this number of elements in the end.
+     195             : 
+     196         197 :   for (size_t vertex_idx = 0; vertex_idx < n_vertices; ++vertex_idx) {
+     197         181 :     const Vertex& vertex = vertices_[vertex_idx];
+     198             : 
+     199             :     // Extract constraints and sort them to fixed and free. For the start and
+     200             :     // end Vertex, we need to do this once, while we need to do it twice for the
+     201             :     // other vertices, since constraints are shared and enforce continuity.
+     202         181 :     int n_constraint_occurence = 2;
+     203         181 :     if (vertex_idx == 0 || vertex_idx == (n_segments_))
+     204          32 :       n_constraint_occurence = 1;
+     205         511 :     for (int co = 0; co < n_constraint_occurence; ++co) {
+     206        1980 :       for (size_t constraint_idx = 0; constraint_idx < N / 2; ++constraint_idx) {
+     207        3300 :         Constraint constraint;
+     208        1650 :         constraint.vertex_idx     = vertex_idx;
+     209        1650 :         constraint.constraint_idx = constraint_idx;
+     210        1650 :         bool has_constraint       = vertex.getConstraint(constraint_idx, &(constraint.value));
+     211        1650 :         if (has_constraint) {
+     212         406 :           all_constraints.push_back(constraint);
+     213         406 :           fixed_constraints.insert(constraint);
+     214             :         } else {
+     215        1244 :           constraint.value = Vertex::ConstraintValue::Constant(dimension_, 0);
+     216        1244 :           all_constraints.push_back(constraint);
+     217        1244 :           free_constraints.insert(constraint);
+     218             :         }
+     219             :       }
+     220             :     }
+     221             :   }
+     222             : 
+     223          16 :   n_all_constraints_   = all_constraints.size();
+     224          16 :   n_fixed_constraints_ = fixed_constraints.size();
+     225          16 :   n_free_constraints_  = free_constraints.size();
+     226             : 
+     227          16 :   reordering_list.reserve(n_all_constraints_);
+     228          16 :   constraint_reordering_ = Eigen::SparseMatrix<double>(n_all_constraints_, n_fixed_constraints_ + n_free_constraints_);
+     229             : 
+     230          80 :   for (Eigen::VectorXd& df : fixed_constraints_compact_)
+     231          64 :     df.resize(n_fixed_constraints_, Eigen::NoChange);
+     232             : 
+     233          16 :   int row = 0;
+     234          16 :   int col = 0;
+     235        1666 :   for (const Constraint& ca : all_constraints) {
+     236       36880 :     for (const Constraint& cf : fixed_constraints) {
+     237       35230 :       if (ca == cf) {
+     238         406 :         reordering_list.emplace_back(Triplet(row, col, 1.0));
+     239        2030 :         for (size_t d = 0; d < dimension_; ++d) {
+     240        1624 :           Eigen::VectorXd&      df                        = fixed_constraints_compact_[d];
+     241        1624 :           const Eigen::VectorXd constraint_all_dimensions = cf.value;
+     242        1624 :           df[col]                                         = constraint_all_dimensions[d];
+     243             :         }
+     244             :       }
+     245       35230 :       ++col;
+     246             :     }
+     247      102220 :     for (const Constraint& cp : free_constraints) {
+     248      100570 :       if (ca == cp)
+     249        1244 :         reordering_list.emplace_back(Triplet(row, col, 1.0));
+     250      100570 :       ++col;
+     251             :     }
+     252        1650 :     col = 0;
+     253        1650 :     ++row;
+     254             :   }
+     255             : 
+     256          16 :   constraint_reordering_.setFromTriplets(reordering_list.begin(), reordering_list.end());
+     257          16 : }
+     258             : 
+     259             : //}
+     260             : 
+     261             : /* setupConstraintReorderingMatrix() //{ */
+     262             : 
+     263             : template <int _N>
+     264        1933 : void PolynomialOptimization<_N>::updateSegmentsFromCompactConstraints() {
+     265        1933 :   const size_t n_all_constraints = n_fixed_constraints_ + n_free_constraints_;
+     266             : 
+     267        9665 :   for (size_t dimension_idx = 0; dimension_idx < dimension_; ++dimension_idx) {
+     268        7732 :     const Eigen::VectorXd& df     = fixed_constraints_compact_[dimension_idx];
+     269        7732 :     const Eigen::VectorXd& dp_opt = free_constraints_compact_[dimension_idx];
+     270             : 
+     271       15464 :     Eigen::VectorXd d_all(n_all_constraints);
+     272        7732 :     d_all << df, dp_opt;
+     273             : 
+     274       82676 :     for (size_t i = 0; i < n_segments_; ++i) {
+     275       74944 :       const Eigen::Matrix<double, N, 1> new_d   = constraint_reordering_.block(i * N, 0, N, n_all_constraints) * d_all;
+     276       74944 :       const Eigen::Matrix<double, N, 1> coeffs  = inverse_mapping_matrices_[i] * new_d;
+     277       74944 :       Segment&                          segment = segments_[i];
+     278       74944 :       segment.setTime(segment_times_[i]);
+     279       74944 :       segment[dimension_idx] = Polynomial(N, coeffs);
+     280             :     }
+     281             :   }
+     282        1933 : }
+     283             : 
+     284             : //}
+     285             : 
+     286             : /* updateSegmentTimes() //{ */
+     287             : 
+     288             : template <int _N>
+     289        1949 : void PolynomialOptimization<_N>::updateSegmentTimes(const std::vector<double>& segment_times) {
+     290        1949 :   const size_t n_segment_times = segment_times.size();
+     291        1949 :   CHECK(n_segment_times == n_segments_) << "Number of segment times (" << n_segment_times << ") does not match number of segments (" << n_segments_ << ")";
+     292             : 
+     293        1949 :   segment_times_ = segment_times;
+     294             : 
+     295       20850 :   for (size_t i = 0; i < n_segments_; i++) {
+     296       18901 :     const double segment_time = segment_times[i];
+     297       18901 :     CHECK_GT(segment_time, 0) << "Segment times need to be greater than zero";
+     298             : 
+     299       18901 :     computeQuadraticCostJacobian(derivative_to_optimize_, segment_time, &cost_matrices_[i]);
+     300       18901 :     SquareMatrix A;
+     301       18901 :     setupMappingMatrix(segment_time, &A);
+     302       18901 :     invertMappingMatrix(A, &inverse_mapping_matrices_[i]);
+     303             :   };
+     304        1949 : }
+     305             : 
+     306             : //}
+     307             : 
+     308             : /* constructR() //{ */
+     309             : 
+     310             : template <int _N>
+     311        1933 : void PolynomialOptimization<_N>::constructR(Eigen::SparseMatrix<double>* R) const {
+     312        1933 :   CHECK_NOTNULL(R);
+     313             :   typedef Eigen::Triplet<double> Triplet;
+     314        3866 :   std::vector<Triplet>           cost_unconstrained_triplets;
+     315        1933 :   cost_unconstrained_triplets.reserve(N * N * n_segments_);
+     316             : 
+     317       20669 :   for (size_t i = 0; i < n_segments_; ++i) {
+     318       18736 :     const SquareMatrix& Ai        = inverse_mapping_matrices_[i];
+     319       18736 :     const SquareMatrix& Q         = cost_matrices_[i];
+     320       18736 :     const SquareMatrix  H         = Ai.transpose() * Q * Ai;
+     321       18736 :     const int           start_pos = i * N;
+     322      206096 :     for (int row = 0; row < N; ++row) {
+     323     2060960 :       for (int col = 0; col < N; ++col) {
+     324     1873600 :         cost_unconstrained_triplets.emplace_back(Triplet(start_pos + row, start_pos + col, H(row, col)));
+     325             :       }
+     326             :     }
+     327             :   }
+     328        1933 :   Eigen::SparseMatrix<double> cost_unconstrained(N * n_segments_, N * n_segments_);
+     329        1933 :   cost_unconstrained.setFromTriplets(cost_unconstrained_triplets.begin(), cost_unconstrained_triplets.end());
+     330             : 
+     331             :   // [1]: R = C^T * H * C. C: constraint_reodering_ ; H: cost_unconstrained,
+     332             :   // assembled from the block-H above.
+     333        1933 :   *R = constraint_reordering_.transpose() * cost_unconstrained * constraint_reordering_;
+     334        1933 : }
+     335             : 
+     336             : //}
+     337             : 
+     338             : /* solveLinear() //{ */
+     339             : 
+     340             : template <int _N>
+     341        1933 : bool PolynomialOptimization<_N>::solveLinear() {
+     342        1933 :   CHECK(derivative_to_optimize_ >= 0 && derivative_to_optimize_ <= kHighestDerivativeToOptimize);
+     343             :   // Catch the fully constrained case:
+     344        1933 :   if (n_free_constraints_ == 0) {
+     345           0 :     DLOG(WARNING) << "No free constraints set in the vertices. Polynomial can "
+     346             :                      "not be optimized. Outputting fully constrained polynomial.";
+     347           0 :     updateSegmentsFromCompactConstraints();
+     348           0 :     return true;
+     349             :   }
+     350             : 
+     351             :   // TODO(acmarkus): figure out if sparse becomes less efficient for small
+     352             :   // problems, and switch back to dense in case.
+     353             : 
+     354             :   // Compute cost matrix for the unconstrained optimization problem.
+     355             :   // Block-wise H = A^{-T}QA^{-1} according to [1]
+     356        3866 :   Eigen::SparseMatrix<double> R;
+     357        1933 :   constructR(&R);
+     358             : 
+     359             :   // Extract block matrices and prepare solver.
+     360        3866 :   Eigen::SparseMatrix<double> Rpf = R.block(n_fixed_constraints_, 0, n_free_constraints_, n_fixed_constraints_);
+     361        3866 :   Eigen::SparseMatrix<double> Rpp = R.block(n_fixed_constraints_, n_fixed_constraints_, n_free_constraints_, n_free_constraints_);
+     362        1933 :   Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> solver;
+     363        1933 :   solver.compute(Rpp);
+     364             : 
+     365             :   // Compute dp_opt for every dimension.
+     366        9665 :   for (size_t dimension_idx = 0; dimension_idx < dimension_; ++dimension_idx) {
+     367        7732 :     Eigen::VectorXd df                       = -Rpf * fixed_constraints_compact_[dimension_idx];  // Rpf = Rfp^T
+     368        7732 :     free_constraints_compact_[dimension_idx] = solver.solve(df);                                  // dp = -Rpp^-1 * Rpf * df
+     369             :   }
+     370             : 
+     371        1933 :   updateSegmentsFromCompactConstraints();
+     372        1933 :   return true;
+     373             : }
+     374             : 
+     375             : //}
+     376             : 
+     377             : /* printReorderingMatrix() //{ */
+     378             : 
+     379             : template <int _N>
+     380             : void PolynomialOptimization<_N>::printReorderingMatrix(std::ostream& stream) const {
+     381             :   stream << "Mapping matrix:\n" << constraint_reordering_ << std::endl;
+     382             : }
+     383             : 
+     384             : //}
+     385             : 
+     386             : /* computeSegmentMaximumMagnitudeCandidates() //{ */
+     387             : 
+     388             : template <int _N>
+     389             : template <int Derivative>
+     390             : bool PolynomialOptimization<_N>::computeSegmentMaximumMagnitudeCandidates(const Segment& segment, double t_start, double t_stop,
+     391             :                                                                           std::vector<double>* candidates) {
+     392             :   return computeSegmentMaximumMagnitudeCandidates(Derivative, segment, t_start, t_stop, candidates);
+     393             : }
+     394             : 
+     395             : //}
+     396             : 
+     397             : /* computeSegmentMaximumMagnitudeCandidates() //{ */
+     398             : 
+     399             : template <int _N>
+     400           0 : bool PolynomialOptimization<_N>::computeSegmentMaximumMagnitudeCandidates(int derivative, const Segment& segment, double t_start, double t_stop,
+     401             :                                                                           std::vector<double>* candidates) {
+     402           0 :   CHECK(candidates);
+     403           0 :   CHECK(N - derivative - 1 > 0) << "N-Derivative-1 has to be greater 0";
+     404             : 
+     405             :   // Use the implementation of this in the segment (template-free) as it's
+     406             :   // actually faster.
+     407           0 :   std::vector<int> dimensions(segment.D());
+     408           0 :   std::iota(dimensions.begin(), dimensions.end(), 0);
+     409           0 :   return segment.computeMinMaxMagnitudeCandidateTimes(derivative, t_start, t_stop, dimensions, candidates);
+     410             : }
+     411             : 
+     412             : //}
+     413             : 
+     414             : /* computeSegmentMaximumMagnitudeCandidatesBySampling() //{ */
+     415             : 
+     416             : template <int _N>
+     417             : template <int Derivative>
+     418             : void PolynomialOptimization<_N>::
+     419             :     computeSegmentMaximumMagnitudeCandidatesBySampling(
+     420             :         const Segment& segment, double t_start, double t_stop, double dt,
+     421             :         std::vector<double>* candidates) {
+     422             :   CHECK_NOTNULL(candidates);
+     423             :   // Start is candidate.
+     424             :   candidates->push_back(t_start);
+     425             : 
+     426             :   // Determine initial direction from t_start to t_start + dt.
+     427             :   auto t_old = t_start + dt;
+     428             :   auto value_new = segment.evaluate(t_old, Derivative);
+     429             :   auto value_old = segment.evaluate(t_start, Derivative);
+     430             :   auto direction = value_new.norm() - value_old.norm();
+     431             : 
+     432             :   // Continue with direction from t_start + dt to t_start + 2 dt until t_stop.
+     433             :   bool last_sample = false;
+     434             :   for (double t = t_start + dt + dt; t <= t_stop; t += dt) {
+     435             :     // Update direction.
+     436             :     value_old = value_new;
+     437             :     value_new = segment.evaluate(t, Derivative);
+     438             :     auto direction_new = value_new.norm() - value_old.norm();
+     439             : 
+     440             :     if (std::signbit(direction) != std::signbit(direction_new)) {
+     441             :       auto value_deriv = segment.evaluate(t_old, Derivative + 1);
+     442             :       if (value_deriv.norm() < 1e-2) {
+     443             :         candidates->push_back(t_old);  // extremum was at last dt
+     444             :       }
+     445             :     }
+     446             : 
+     447             :     direction = direction_new;
+     448             :     t_old = t;
+     449             : 
+     450             :     // Check last sample before t_stop.
+     451             :     if ((t + dt) > t_stop && !last_sample) {
+     452             :       t = t_stop - dt;
+     453             :       last_sample = true;
+     454             :     }
+     455             :   }
+     456             : 
+     457             :   // End is candidates.
+     458             :   if (candidates->back() != t_stop) {
+     459             :     candidates->push_back(t_stop);
+     460             :   }
+     461             : }
+     462             : 
+     463             : //}
+     464             : 
+     465             : /* computeMaximumOfMagnitude() //{ */
+     466             : 
+     467             : template <int _N>
+     468             : template <int Derivative>
+     469             : Extremum PolynomialOptimization<_N>::computeMaximumOfMagnitude(std::vector<Extremum>* candidates) const {
+     470             :   return computeMaximumOfMagnitude(Derivative, candidates);
+     471             : }
+     472             : 
+     473             : //}
+     474             : 
+     475             : /* computeMaximumOfMagnitude() //{ */
+     476             : 
+     477             : template <int _N>
+     478           0 : Extremum PolynomialOptimization<_N>::computeMaximumOfMagnitude(int derivative, std::vector<Extremum>* candidates) const {
+     479           0 :   if (candidates != nullptr)
+     480           0 :     candidates->clear();
+     481             : 
+     482           0 :   int      segment_idx = 0;
+     483           0 :   Extremum extremum;
+     484           0 :   for (const Segment& s : segments_) {
+     485           0 :     std::vector<double> extrema_times;
+     486           0 :     extrema_times.reserve(N - 1);
+     487             :     // Add the beginning as well. Call below appends its extrema.
+     488           0 :     extrema_times.push_back(0.0);
+     489           0 :     computeSegmentMaximumMagnitudeCandidates(derivative, s, 0.0, s.getTime(), &extrema_times);
+     490             : 
+     491           0 :     for (double t : extrema_times) {
+     492           0 :       const Extremum candidate(t, s.evaluate(t, derivative).norm(), segment_idx);
+     493           0 :       if (extremum < candidate)
+     494           0 :         extremum = candidate;
+     495           0 :       if (candidates != nullptr)
+     496           0 :         candidates->emplace_back(candidate);
+     497             :     }
+     498           0 :     ++segment_idx;
+     499             :   }
+     500             :   // Check last time at last segment.
+     501           0 :   const Extremum candidate(segments_.back().getTime(), segments_.back().evaluate(segments_.back().getTime(), derivative).norm(), n_segments_ - 1);
+     502           0 :   if (extremum < candidate)
+     503           0 :     extremum = candidate;
+     504           0 :   if (candidates != nullptr)
+     505           0 :     candidates->emplace_back(candidate);
+     506             : 
+     507           0 :   return extremum;
+     508             : }
+     509             : 
+     510             : //}
+     511             : 
+     512             : /* setFreeConstraints() //{ */
+     513             : 
+     514             : template <int _N>
+     515           0 : void PolynomialOptimization<_N>::setFreeConstraints(const std::vector<Eigen::VectorXd>& free_constraints) {
+     516           0 :   CHECK(free_constraints.size() == dimension_);
+     517           0 :   for (const Eigen::VectorXd& v : free_constraints)
+     518           0 :     CHECK(static_cast<size_t>(v.size()) == n_free_constraints_);
+     519             : 
+     520           0 :   free_constraints_compact_ = free_constraints;
+     521           0 :   updateSegmentsFromCompactConstraints();
+     522           0 : }
+     523             : 
+     524             : //}
+     525             : 
+     526             : /* getAInverse() //{ */
+     527             : 
+     528             : template <int _N>
+     529             : void PolynomialOptimization<_N>::getAInverse(Eigen::MatrixXd* A_inv) const {
+     530             :   CHECK_NOTNULL(A_inv);
+     531             : 
+     532             :   A_inv->resize(N * n_segments_, N * n_segments_);
+     533             :   A_inv->setZero();
+     534             : 
+     535             :   for (size_t i = 0; i < n_segments_; ++i) {
+     536             :     (*A_inv).block<N, N>(N * i, N * i) = inverse_mapping_matrices_[i];
+     537             :   }
+     538             : }
+     539             : 
+     540             : //}
+     541             : 
+     542             : /* getM() //{ */
+     543             : 
+     544             : template <int _N>
+     545             : void PolynomialOptimization<_N>::getM(Eigen::MatrixXd* M) const {
+     546             :   CHECK_NOTNULL(M);
+     547             :   *M = constraint_reordering_;
+     548             : }
+     549             : 
+     550             : //}
+     551             : 
+     552             : /* getR() //{ */
+     553             : 
+     554             : template <int _N>
+     555             : void PolynomialOptimization<_N>::getR(Eigen::MatrixXd* R) const {
+     556             :   CHECK_NOTNULL(R);
+     557             : 
+     558             :   Eigen::SparseMatrix<double> R_sparse;
+     559             :   constructR(&R_sparse);
+     560             : 
+     561             :   *R = R_sparse;
+     562             : }
+     563             : 
+     564             : //}
+     565             : 
+     566             : /* getA() //{ */
+     567             : 
+     568             : template <int _N>
+     569             : void PolynomialOptimization<_N>::getA(Eigen::MatrixXd* A) const {
+     570             :   CHECK_NOTNULL(A);
+     571             :   A->resize(N * n_segments_, N * n_segments_);
+     572             :   A->setZero();
+     573             : 
+     574             :   // Create a mapping matrix per segment and append them together.
+     575             :   for (size_t i = 0; i < n_segments_; ++i) {
+     576             :     const double segment_time = segment_times_[i];
+     577             :     CHECK_GT(segment_time, 0) << "Segment times need to be greater than zero";
+     578             : 
+     579             :     SquareMatrix A_segment;
+     580             :     setupMappingMatrix(segment_time, &A_segment);
+     581             : 
+     582             :     (*A).block<N, N>(N * i, N * i) = A_segment;
+     583             :   }
+     584             : }
+     585             : 
+     586             : //}
+     587             : 
+     588             : /* getMpinv() //{ */
+     589             : 
+     590             : template <int _N>
+     591             : void PolynomialOptimization<_N>::getMpinv(Eigen::MatrixXd* M_pinv) const {
+     592             :   CHECK_NOTNULL(M_pinv);
+     593             : 
+     594             :   // Pseudoinverse implementation by @SebastianInd.
+     595             :   *M_pinv = constraint_reordering_.transpose();
+     596             :   for (int M_row = 0; M_row < M_pinv->rows(); M_row++) {
+     597             :     M_pinv->row(M_row) = M_pinv->row(M_row) / M_pinv->row(M_row).sum();
+     598             :   }
+     599             : }
+     600             : 
+     601             : //}
+     602             : 
+     603             : /* computeQuadraticCostJacobian() //{ */
+     604             : 
+     605             : template <int _N>
+     606       18901 : void PolynomialOptimization<_N>::computeQuadraticCostJacobian(int derivative, double t, SquareMatrix* cost_jacobian) {
+     607       18901 :   CHECK_LT(derivative, N);
+     608             : 
+     609       18901 :   cost_jacobian->setZero();
+     610      170109 :   for (int col = 0; col < N - derivative; col++) {
+     611     1360870 :     for (int row = 0; row < N - derivative; row++) {
+     612     1209660 :       double exponent = (N - 1 - derivative) * 2 + 1 - row - col;
+     613             : 
+     614     1209660 :       (*cost_jacobian)(N - 1 - row, N - 1 - col) =
+     615     1209660 :           Polynomial::base_coefficients_(derivative, N - 1 - row) * Polynomial::base_coefficients_(derivative, N - 1 - col) * pow(t, exponent) * 2.0 / exponent;
+     616             :     }
+     617             :   }
+     618       18901 : }
+     619             : 
+     620             : //}
+     621             : 
+     622             : }  // namespace eth_trajectory_generation
+     623             : 
+     624             : #endif  // eth_trajectory_generation_IMPL_POLYNOMIAL_OPTIMIZATION_LINEAR_IMPL_H_
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.overview.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.overview.html new file mode 100644 index 0000000000..632398b089 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.overview.html @@ -0,0 +1,176 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.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 + + +
    + Top

    + Overview +
    + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..06feb6795c75b3a2e451e1d769acc07c149b8979 GIT binary patch literal 2764 zcmV;-3N!VIP)=lv?oc&k10X+~PKqPMaxA$-!j^i%^Bq4+lUbe+p)^&T8 z_4OzGb>YV0ef+m=DiFa6t^mTb^#yht(%+l{?BKLEHh@#Tb{i7G8@Q`FLw_9BzGggf zIdF5~3AIhFda8|8aSCvTB5=6|nlIS+x~7WpzzOC~mN5 zS5UMou5T)?7uS;Ee&6LU)6H_f-=B}?`F#C-Jj4Gde83WJ;w=JSpW*2vJ&KnXg-5uY z0eFWm!n5;yrq4j~4#mzM*_mAq$U<8HNMG)%hUNOmz>n4JRA&S_&{T(RAEf zMFLN-IB1&KTAlI9l7905^Yn-o=z%|dkSz+pV6mE2a@C=swMy2U3P983wU~V=V&l=9 zgOQKO!&m8b=GEz8kHIusXSJL+oi^4?>;Qd_6ameyg1LODt8j^jdy%U^kI+>J*4&F} z!{bm6-+{FUKK7U2MYq>gtoIwfaI`4?WkN#y-djknITz!6bel8wKDO!x5{iHKYSjKV z(|pmm`6|;KZ}#|yb*|*&%iKa4cl%EPjZ^9dfo?Yx%@!+l;~^14nJdRHw~Zs+MWePg z-!xfT;|+kJ+q^7Mfwu!yL7D~K09Z#M?_h8+mbL*zw=h0yashihl+wvs7hvX6jN2EcILVcr=750KHX@_Vkcf2YwVHQI_jr^zXEqi7oeY2A~=5 zq0vKaO!lD{@|lhIaHIYdpKVWyg*wvd6|eBMUAYRjY)#)5SYxEg0J!X{>1ta~lk znz6eL%w4CCrVp+3fi0bQ$Kpc!OOi!rifS6u++5Y;;KMBN5GIlsQUe>v5{eOazxS*J z0aYo&47e9Sa+Ez!ZuQdsCP{{m@~N;JNcBPz__vP#KRkjrt-p8ZAE>}Qfg^xTk|J*E78mJ`Bjg193N-Zifd8Vnaz z$1rk>;gnej`GT0DER<D_Z6jJ~`_UA(Ru1xrNWVD2aCfd~{Rx5NJ!=n@Fino+d{A z`Fs|Q5jrX)?s@OQbd0E9^@>@zYf?|zgWfV=9NOubQ}+Bz+tVI}1R0IO!Se*hxXQa~V!yE0E@dIBqMlmX*V6O8?0&-;PUbpw=37EdO;74C zrm$W#hnT02#$mUtJ&6Wc}iA8p6c%*Y=4?27CcmmK^(s^JS z$SK7RN*MHsy9_H@7#hDq9( z;+hVKq|f+;~zC z9T|Hv82vz!`aR(24+>`Gx7Q4Ttc|YJ=^UAtCdD7@^jEtI7px3{%(bq9ZK2Xyebh73 zKgo1U=~-6=1EtHIKAruAMHWo$dd)`fIymfSd_hHArgWTgSOPpu3!f8_C#`>4ZLAg) zLttD}wVJu$OF8I(dwdPY6W3Y~v9zgz(mjf8>767&>jDxfo`N^cWA|{5Jst2T9`V|X zKH`Rc^7~kfYnjhL@hbHb5YBi#l~*# z%n)~f&j7U9J!b$+!}|W)R_x!MG1N2lr}$jQQgC=Hk|9GHpPwn}QUYtG$X;6{fJPB! z1e+Dtfgv4rpV1z38~@baaNje-#mh=0x3RM~*~l#|&*;c)J$kOLSOg2GQa1`Ax%e&( zh%6LdYb<|lQE_3~)pCtK#!YW0kVd7*CDz9Mlgt2kxn!mqnaodzwn@-)RfF~nasRYu z0K9%&N7QigAc|LVGcuC0Ow#+4IiQ_@6Y~shbY!9AR&R)N7)Ez|vvv~dTD<{cV$T(Xic!d0P%D#cs^EK#%reYUsJm)(J^-jFeZ%3lf&n31tfMB0 zm2>~|DvK zF~yEg@5T~y!{<9A((ED?ctfAo## + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl - polynomial_optimization_nonlinear_impl.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:14742934.3 %
    Date:2024-04-09 22:03:33Functions:81942.1 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTime()0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::objectiveFunctionTime(std::vector<double, std::allocator<double> > const&, std::vector<double, std::allocator<double> >&, void*)0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::computeTotalTrajectoryTime(std::vector<double, std::allocator<double> > const&)0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeAndFreeConstraints()0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::evaluateMaximumMagnitudeConstraint(std::vector<double, std::allocator<double> > const&, std::vector<double, std::allocator<double> >&, void*)0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::objectiveFunctionTimeAndConstraints(std::vector<double, std::allocator<double> > const&, std::vector<double, std::allocator<double> >&, void*)0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::setFreeEndpointDerivativeHardConstraints(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&, std::vector<double, std::allocator<double> >*, std::vector<double, std::allocator<double> >*)0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::evaluateMaximumMagnitudeAsSoftConstraint(std::vector<std::shared_ptr<eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::ConstraintData>, std::allocator<std::shared_ptr<eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::ConstraintData> > > const&, double, double) const0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()::{lambda(double)#2}::operator()(double) const0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()::{lambda(double)#3}::operator()(double) const0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()::{lambda(double)#1}::operator()(double) const0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::setupFromVertices(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&, std::vector<double, std::allocator<double> > const&, int)16
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()16
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::scaleSegmentTimesWithViolation()16
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimize()16
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::PolynomialOptimizationNonLinear(unsigned long, eth_trajectory_generation::NonlinearOptimizationParameters const&)16
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::addMaximumMagnitudeConstraint(int, int, double)192
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::getCostAndGradientMellinger(std::vector<double, std::allocator<double> >*)211
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::objectiveFunctionTimeMellingerOuterLoop(std::vector<double, std::allocator<double> > const&, std::vector<double, std::allocator<double> >&, void*)211
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.func.html new file mode 100644 index 0000000000..cfcefec071 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.func.html @@ -0,0 +1,156 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl - polynomial_optimization_nonlinear_impl.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:14742934.3 %
    Date:2024-04-09 22:03:33Functions:81942.1 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTime()0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::setupFromVertices(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&, std::vector<double, std::allocator<double> > const&, int)16
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::objectiveFunctionTime(std::vector<double, std::allocator<double> > const&, std::vector<double, std::allocator<double> >&, void*)0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::computeTotalTrajectoryTime(std::vector<double, std::allocator<double> > const&)0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::getCostAndGradientMellinger(std::vector<double, std::allocator<double> >*)211
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::addMaximumMagnitudeConstraint(int, int, double)192
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeAndFreeConstraints()0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()16
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::scaleSegmentTimesWithViolation()16
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::evaluateMaximumMagnitudeConstraint(std::vector<double, std::allocator<double> > const&, std::vector<double, std::allocator<double> >&, void*)0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::objectiveFunctionTimeAndConstraints(std::vector<double, std::allocator<double> > const&, std::vector<double, std::allocator<double> >&, void*)0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::objectiveFunctionTimeMellingerOuterLoop(std::vector<double, std::allocator<double> > const&, std::vector<double, std::allocator<double> >&, void*)211
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::setFreeEndpointDerivativeHardConstraints(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&, std::vector<double, std::allocator<double> >*, std::vector<double, std::allocator<double> >*)0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimize()16
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::PolynomialOptimizationNonLinear(unsigned long, eth_trajectory_generation::NonlinearOptimizationParameters const&)16
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::evaluateMaximumMagnitudeAsSoftConstraint(std::vector<std::shared_ptr<eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::ConstraintData>, std::allocator<std::shared_ptr<eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::ConstraintData> > > const&, double, double) const0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()::{lambda(double)#2}::operator()(double) const0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()::{lambda(double)#3}::operator()(double) const0
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::optimizeTimeMellingerOuterLoop()::{lambda(double)#1}::operator()(double) const0
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.frameset.html new file mode 100644 index 0000000000..a8e09e9987 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.html new file mode 100644 index 0000000000..74731a46bb --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.html @@ -0,0 +1,933 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl - polynomial_optimization_nonlinear_impl.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:14742934.3 %
    Date:2024-04-09 22:03:33Functions:81942.1 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright (c) 2015, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
+       4             :  *
+       5             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       6             :  * you may not use this file except in compliance with the License.
+       7             :  * You may obtain a copy of the License at
+       8             :  *
+       9             :  * http://www.apache.org/licenses/LICENSE-2.0
+      10             :  *
+      11             :  * Unless required by applicable law or agreed to in writing, software
+      12             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      13             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      14             :  * See the License for the specific language governing permissions and
+      15             :  * limitations under the License.
+      16             :  */
+      17             : 
+      18             : #ifndef ETH_TRAJECTORY_GENERATION_IMPL_POLYNOMIAL_OPTIMIZATION_NONLINEAR_IMPL_H_
+      19             : #define ETH_TRAJECTORY_GENERATION_IMPL_POLYNOMIAL_OPTIMIZATION_NONLINEAR_IMPL_H_
+      20             : 
+      21             : #include <chrono>
+      22             : #include <numeric>
+      23             : 
+      24             : #include <eth_trajectory_generation/polynomial_optimization_nonlinear.h>
+      25             : #include <eth_trajectory_generation/timing.h>
+      26             : 
+      27             : namespace eth_trajectory_generation
+      28             : {
+      29             : 
+      30             : inline std::ostream& operator<<(std::ostream& stream, const OptimizationInfo& val) {
+      31             :   stream << "--- optimization info ---" << std::endl;
+      32             :   stream << "  optimization time:     " << val.optimization_time << std::endl;
+      33             :   stream << "  n_iterations:          " << val.n_iterations << std::endl;
+      34             :   stream << "  stopping reason:       " << nlopt::returnValueToString(val.stopping_reason) << std::endl;
+      35             :   stream << "  cost trajectory:       " << val.cost_trajectory << std::endl;
+      36             :   stream << "  cost time:             " << val.cost_time << std::endl;
+      37             :   stream << "  cost soft constraints: " << val.cost_soft_constraints << std::endl;
+      38             :   stream << "  maxima: " << std::endl;
+      39             :   for (const std::pair<int, Extremum>& m : val.maxima) {
+      40             :     stream << "    " << positionDerivativeToString(m.first) << ": " << m.second.value << " in segment " << m.second.segment_idx << " and segment time "
+      41             :            << m.second.time << std::endl;
+      42             :   }
+      43             :   return stream;
+      44             : }
+      45             : 
+      46             : template <int _N>
+      47          16 : PolynomialOptimizationNonLinear<_N>::PolynomialOptimizationNonLinear(size_t dimension, const NonlinearOptimizationParameters& parameters)
+      48          16 :     : poly_opt_(dimension), optimization_parameters_(parameters) {
+      49          16 : }
+      50             : 
+      51             : template <int _N>
+      52          16 : bool PolynomialOptimizationNonLinear<_N>::setupFromVertices(const Vertex::Vector& vertices, const std::vector<double>& segment_times,
+      53             :                                                             int derivative_to_optimize) {
+      54          16 :   bool ret = poly_opt_.setupFromVertices(vertices, segment_times, derivative_to_optimize);
+      55             : 
+      56             :   size_t n_optimization_parameters;
+      57          16 :   switch (optimization_parameters_.time_alloc_method) {
+      58          16 :     case NonlinearOptimizationParameters::kSquaredTime:
+      59             :     case NonlinearOptimizationParameters::kRichterTime:
+      60             :     case NonlinearOptimizationParameters::kMellingerOuterLoop:
+      61          16 :       n_optimization_parameters = segment_times.size();
+      62          16 :       break;
+      63           0 :     default:
+      64           0 :       n_optimization_parameters = segment_times.size() + poly_opt_.getNumberFreeConstraints() * poly_opt_.getDimension();
+      65           0 :       break;
+      66             :   }
+      67             : 
+      68          16 :   nlopt_.reset(new nlopt::opt(optimization_parameters_.algorithm, n_optimization_parameters));
+      69          16 :   nlopt_->set_ftol_rel(optimization_parameters_.f_rel);
+      70          16 :   nlopt_->set_ftol_abs(optimization_parameters_.f_abs);
+      71          16 :   nlopt_->set_xtol_rel(optimization_parameters_.x_rel);
+      72          16 :   nlopt_->set_xtol_abs(optimization_parameters_.x_abs);
+      73          16 :   nlopt_->set_maxeval(optimization_parameters_.max_iterations);
+      74          16 :   nlopt_->set_maxtime(optimization_parameters_.max_time);
+      75             : 
+      76          16 :   if (optimization_parameters_.random_seed < 0)
+      77           0 :     nlopt_srand_time();
+      78             :   else
+      79          16 :     nlopt_srand(optimization_parameters_.random_seed);
+      80             : 
+      81          16 :   return ret;
+      82             : }
+      83             : 
+      84             : template <int _N>
+      85             : bool PolynomialOptimizationNonLinear<_N>::solveLinear() {
+      86             :   return poly_opt_.solveLinear();
+      87             : }
+      88             : 
+      89             : template <int _N>
+      90          16 : int PolynomialOptimizationNonLinear<_N>::optimize() {
+      91          16 :   optimization_info_ = OptimizationInfo();
+      92          16 :   int result         = nlopt::FAILURE;
+      93             : 
+      94          16 :   const std::chrono::high_resolution_clock::time_point t_start = std::chrono::high_resolution_clock::now();
+      95             : 
+      96          16 :   switch (optimization_parameters_.time_alloc_method) {
+      97           0 :     case NonlinearOptimizationParameters::kSquaredTime:
+      98             :     case NonlinearOptimizationParameters::kRichterTime:
+      99           0 :       result = optimizeTime();
+     100           0 :       break;
+     101           0 :     case NonlinearOptimizationParameters::kSquaredTimeAndConstraints:
+     102             :     case NonlinearOptimizationParameters::kRichterTimeAndConstraints:
+     103           0 :       result = optimizeTimeAndFreeConstraints();
+     104           0 :       break;
+     105          16 :     case NonlinearOptimizationParameters::kMellingerOuterLoop:
+     106          16 :       result = optimizeTimeMellingerOuterLoop();
+     107          16 :       break;
+     108           0 :     default:
+     109           0 :       break;
+     110             :   }
+     111             : 
+     112          16 :   const std::chrono::high_resolution_clock::time_point t_stop = std::chrono::high_resolution_clock::now();
+     113          16 :   optimization_info_.optimization_time                        = std::chrono::duration_cast<std::chrono::duration<double>>(t_stop - t_start).count();
+     114             : 
+     115          16 :   optimization_info_.stopping_reason = result;
+     116             : 
+     117          16 :   return result;
+     118             : }
+     119             : 
+     120             : template <int _N>
+     121           0 : int PolynomialOptimizationNonLinear<_N>::optimizeTime() {
+     122           0 :   std::vector<double> initial_step, segment_times;
+     123             : 
+     124           0 :   poly_opt_.getSegmentTimes(&segment_times);
+     125           0 :   const size_t n_segments = segment_times.size();
+     126             : 
+     127           0 :   initial_step.reserve(n_segments);
+     128           0 :   for (double t : segment_times) {
+     129           0 :     initial_step.push_back(optimization_parameters_.initial_stepsize_rel * t);
+     130             :   }
+     131             : 
+     132             :   try {
+     133             :     // Set a lower bound on the segment time per segment to avoid numerical
+     134             :     // issues.
+     135           0 :     nlopt_->set_initial_step(initial_step);
+     136           0 :     nlopt_->set_upper_bounds(std::numeric_limits<double>::max());
+     137           0 :     nlopt_->set_lower_bounds(kOptimizationTimeLowerBound);
+     138           0 :     nlopt_->set_min_objective(&PolynomialOptimizationNonLinear<N>::objectiveFunctionTime, this);
+     139             :   }
+     140           0 :   catch (std::exception& e) {
+     141           0 :     LOG(ERROR) << "error while setting up nlopt: " << e.what() << std::endl;
+     142           0 :     return nlopt::FAILURE;
+     143             :   }
+     144             : 
+     145           0 :   double final_cost = std::numeric_limits<double>::max();
+     146             :   int    result;
+     147             : 
+     148             :   try {
+     149           0 :     result = nlopt_->optimize(segment_times, final_cost);
+     150             :   }
+     151           0 :   catch (std::exception& e) {
+     152           0 :     LOG(ERROR) << "error while running nlopt: " << e.what() << std::endl;
+     153           0 :     return nlopt::FAILURE;
+     154             :   }
+     155             : 
+     156           0 :   return result;
+     157             : }
+     158             : 
+     159             : template <int _N>
+     160          16 : int PolynomialOptimizationNonLinear<_N>::optimizeTimeMellingerOuterLoop() {
+     161          32 :   std::vector<double> segment_times;
+     162          16 :   poly_opt_.getSegmentTimes(&segment_times);
+     163             : 
+     164             :   // Save original segment times
+     165          32 :   std::vector<double> original_segment_times = segment_times;
+     166             : 
+     167          16 :   if (optimization_parameters_.print_debug_info_time_allocation) {
+     168           0 :     std::cout << "Segment times: ";
+     169           0 :     for (const double seg_time : segment_times) {
+     170           0 :       std::cout << seg_time << " ";
+     171             :     }
+     172           0 :     std::cout << std::endl;
+     173             :   }
+     174             : 
+     175             :   try {
+     176             :     // Set a lower bound on the segment time per segment to avoid numerical
+     177             :     // issues.
+     178          16 :     nlopt_->set_upper_bounds(std::numeric_limits<double>::max());
+     179          16 :     nlopt_->set_lower_bounds(kOptimizationTimeLowerBound);
+     180          16 :     nlopt_->set_min_objective(&PolynomialOptimizationNonLinear<N>::objectiveFunctionTimeMellingerOuterLoop, this);
+     181             :   }
+     182           0 :   catch (std::exception& e) {
+     183           0 :     LOG(ERROR) << "error while setting up nlopt: " << e.what() << std::endl;
+     184           0 :     return nlopt::FAILURE;
+     185             :   }
+     186             : 
+     187          16 :   double final_cost = std::numeric_limits<double>::max();
+     188          16 :   int    result     = nlopt::FAILURE;
+     189             : 
+     190             :   try {
+     191          16 :     result = nlopt_->optimize(segment_times, final_cost);
+     192             :   }
+     193          12 :   catch (std::exception& e) {
+     194           6 :     LOG(ERROR) << "error while running nlopt: " << e.what() << ". This likely means the optimization aborted early." << std::endl;
+     195           6 :     if (final_cost == std::numeric_limits<double>::max()) {
+     196           0 :       return nlopt::FAILURE;
+     197             :     }
+     198             : 
+     199           6 :     if (optimization_parameters_.print_debug_info_time_allocation) {
+     200           0 :       std::cout << "Segment times after opt: ";
+     201           0 :       for (const double seg_time : segment_times) {
+     202           0 :         std::cout << seg_time << " ";
+     203             :       }
+     204           0 :       std::cout << std::endl;
+     205           0 :       std::cout << "Final cost: " << final_cost << std::endl;
+     206           0 :       std::cout << "Nlopt result: " << result << std::endl;
+     207             :     }
+     208             :   }
+     209             : 
+     210             :   // Scaling of segment times
+     211          32 :   std::vector<double> relative_segment_times;
+     212          16 :   poly_opt_.getSegmentTimes(&relative_segment_times);
+     213          16 :   scaleSegmentTimesWithViolation();
+     214          16 :   std::vector<double> scaled_segment_times;
+     215          16 :   poly_opt_.getSegmentTimes(&scaled_segment_times);
+     216             : 
+     217             :   // Print all parameter after scaling
+     218          16 :   if (optimization_parameters_.print_debug_info_time_allocation) {
+     219           0 :     std::cout << "[MEL          Original]: ";
+     220           0 :     std::for_each(original_segment_times.cbegin(), original_segment_times.cend(), [](double c) { std::cout << c << " "; });
+     221           0 :     std::cout << std::endl;
+     222           0 :     std::cout << "[MEL RELATIVE Solution]: ";
+     223           0 :     std::for_each(relative_segment_times.cbegin(), relative_segment_times.cend(), [](double c) { std::cout << c << " "; });
+     224           0 :     std::cout << std::endl;
+     225           0 :     std::cout << "[MEL          Solution]: ";
+     226           0 :     std::for_each(scaled_segment_times.cbegin(), scaled_segment_times.cend(), [](double c) { std::cout << c << " "; });
+     227           0 :     std::cout << std::endl;
+     228           0 :     std::cout << "[MEL   Trajectory Time] Before: " << std::accumulate(original_segment_times.begin(), original_segment_times.end(), 0.0)
+     229           0 :               << " | After Rel Change: " << std::accumulate(relative_segment_times.begin(), relative_segment_times.end(), 0.0)
+     230           0 :               << " | After Scaling: " << std::accumulate(scaled_segment_times.begin(), scaled_segment_times.end(), 0.0) << std::endl;
+     231             :   }
+     232             : 
+     233          16 :   return result;
+     234             : }
+     235             : 
+     236             : template <int _N>
+     237             : double PolynomialOptimizationNonLinear<_N>::getCost() const {
+     238             :   return poly_opt_.computeCost();
+     239             : }
+     240             : 
+     241             : template <int _N>
+     242             : double PolynomialOptimizationNonLinear<_N>::getTotalCostWithSoftConstraints() const {
+     243             :   double cost_trajectory = poly_opt_.computeCost();
+     244             : 
+     245             :   // Use consistent cost metrics regardless of method set, to compare between
+     246             :   // methods.
+     247             :   std::vector<double> segment_times;
+     248             :   poly_opt_.getSegmentTimes(&segment_times);
+     249             :   double total_time       = std::accumulate(segment_times.begin(), segment_times.end(), 0.0);
+     250             :   double cost_time        = total_time * total_time * optimization_parameters_.time_penalty;
+     251             :   double cost_constraints = evaluateMaximumMagnitudeAsSoftConstraint(inequality_constraints_, optimization_parameters_.soft_constraint_weight, 1e9);
+     252             : 
+     253             :   return cost_trajectory + cost_time + cost_constraints;
+     254             : }
+     255             : 
+     256             : template <int _N>
+     257         211 : double PolynomialOptimizationNonLinear<_N>::getCostAndGradientMellinger(std::vector<double>* gradients) {
+     258             :   // Weighting terms for different costs
+     259             :   // Retrieve the current segment times
+     260         422 :   std::vector<double> segment_times;
+     261         211 :   poly_opt_.getSegmentTimes(&segment_times);
+     262         211 :   const double J_d = poly_opt_.computeCost();
+     263             : 
+     264         211 :   if (poly_opt_.getNumberSegments() == 1) {
+     265           0 :     if (gradients != NULL) {
+     266           0 :       gradients->clear();
+     267           0 :       gradients->resize(poly_opt_.getNumberSegments(), 0.0);
+     268             :     }
+     269             : 
+     270           0 :     return J_d;
+     271             :   }
+     272             : 
+     273         211 :   if (gradients != NULL) {
+     274         211 :     const size_t n_segments = poly_opt_.getNumberSegments();
+     275             : 
+     276         211 :     gradients->clear();
+     277         211 :     gradients->resize(n_segments);
+     278             : 
+     279             :     // Initialize changed segment times for numerical derivative
+     280         422 :     std::vector<double> segment_times_bigger(n_segments);
+     281         211 :     const double        increment_time = 0.1;
+     282        1706 :     for (size_t n = 0; n < n_segments; ++n) {
+     283             :       // Now the same with an increased segment time
+     284             :       // Calculate cost with higher segment time
+     285        1495 :       segment_times_bigger = segment_times;
+     286             :       // Deduct h*(-1/(m-2)) according to paper Mellinger "Minimum snap
+     287             :       // trajectory generation and control for quadrotors"
+     288        1495 :       double const_traj_time_corr = increment_time / (n_segments - 1.0);
+     289       17076 :       for (size_t i = 0; i < segment_times_bigger.size(); ++i) {
+     290       15581 :         if (i == n) {
+     291        1495 :           segment_times_bigger[i] += increment_time;
+     292             :         } else {
+     293       14086 :           segment_times_bigger[i] -= const_traj_time_corr;
+     294             :         }
+     295             :       }
+     296             : 
+     297             :       // TODO: add case if segment_time is at threshold 0.1s
+     298             :       // 1) How many segments > 0.1s
+     299             :       // 2) trajectory time correction only on those
+     300             :       // for (int j = 0; j < segment_times_bigger.size(); ++j) {
+     301             :       //   double thresh_corr = 0.0;
+     302             :       //   if (segment_times_bigger[j] < 0.1) {
+     303             :       //     thresh_corr = 0.1-segment_times_bigger[j];
+     304             :       //   }
+     305             :       // }
+     306             : 
+     307             :       // Check and make sure that segment times are >
+     308             :       // kOptimizationTimeLowerBound
+     309       17076 :       for (double& t : segment_times_bigger) {
+     310       15581 :         t = std::max(kOptimizationTimeLowerBound, t);
+     311             :       }
+     312             : 
+     313             :       // Update the segment times. This changes the polynomial coefficients.
+     314        1495 :       poly_opt_.updateSegmentTimes(segment_times_bigger);
+     315        1495 :       poly_opt_.solveLinear();
+     316             : 
+     317             :       // Calculate cost and gradient with new segment time
+     318        1495 :       const double J_d_bigger = poly_opt_.computeCost();
+     319        1495 :       const double dJd_dt     = (J_d_bigger - J_d) / increment_time;
+     320             : 
+     321             :       // Calculate the gradient
+     322        1495 :       gradients->at(n) = dJd_dt;
+     323             :     }
+     324             : 
+     325             :     // Set again the original segment times from before calculating the
+     326             :     // numerical gradient
+     327         211 :     poly_opt_.updateSegmentTimes(segment_times);
+     328         211 :     poly_opt_.solveLinear();
+     329             :   }
+     330             : 
+     331             :   // Compute cost without gradient
+     332         211 :   return J_d;
+     333             : }
+     334             : 
+     335             : template <int _N>
+     336          16 : void PolynomialOptimizationNonLinear<_N>::scaleSegmentTimesWithViolation() {
+     337             : 
+     338             :   // Get trajectory
+     339          32 :   Trajectory traj;
+     340          16 :   poly_opt_.getTrajectory(&traj);
+     341             : 
+     342             :   // get constraints
+     343          16 :   double v_max_horizontal = 0.0;
+     344          16 :   double a_max_horizontal = 0.0;
+     345          16 :   double j_max_horizontal = 0.0;
+     346             : 
+     347          16 :   double v_max_vertical = 0.0;
+     348          16 :   double a_max_vertical = 0.0;
+     349          16 :   double j_max_vertical = 0.0;
+     350             : 
+     351          16 :   double v_max_heading = 0.0;
+     352          16 :   double a_max_heading = 0.0;
+     353          16 :   double j_max_heading = 0.0;
+     354             : 
+     355         208 :   for (const auto& constraint : inequality_constraints_) {
+     356         192 :     if (constraint->dimension <= 1) {
+     357          96 :       if (constraint->derivative == derivative_order::VELOCITY) {
+     358          32 :         v_max_horizontal = constraint->value;
+     359          64 :       } else if (constraint->derivative == derivative_order::ACCELERATION) {
+     360          32 :         a_max_horizontal = constraint->value;
+     361          32 :       } else if (constraint->derivative == derivative_order::JERK) {
+     362          32 :         j_max_horizontal = constraint->value;
+     363             :       }
+     364          96 :     } else if (constraint->dimension == 2) {
+     365          48 :       if (constraint->derivative == derivative_order::VELOCITY) {
+     366          16 :         v_max_vertical = constraint->value;
+     367          32 :       } else if (constraint->derivative == derivative_order::ACCELERATION) {
+     368          16 :         a_max_vertical = constraint->value;
+     369          16 :       } else if (constraint->derivative == derivative_order::JERK) {
+     370          16 :         j_max_vertical = constraint->value;
+     371             :       }
+     372          48 :     } else if (constraint->dimension == 3) {
+     373          48 :       if (constraint->derivative == derivative_order::VELOCITY) {
+     374          16 :         v_max_heading = constraint->value;
+     375          32 :       } else if (constraint->derivative == derivative_order::ACCELERATION) {
+     376          16 :         a_max_heading = constraint->value;
+     377          16 :       } else if (constraint->derivative == derivative_order::JERK) {
+     378          16 :         j_max_heading = constraint->value;
+     379             :       }
+     380             :     }
+     381             :   }
+     382             : 
+     383          16 :   if (optimization_parameters_.print_debug_info_time_allocation) {
+     384             : 
+     385             :     double v_max_actual_horizontal, a_max_actual_horizontal, j_max_actual_horizontal;
+     386           0 :     traj.computeMaxDerivativesHorizontal(&v_max_actual_horizontal, &a_max_actual_horizontal, &j_max_actual_horizontal);
+     387           0 :     std::cout << "[Time Scaling] Beginning: v: max: " << v_max_actual_horizontal << " / " << v_max_horizontal << " a: max: " << a_max_actual_horizontal << " / "
+     388           0 :               << a_max_horizontal << std::endl;
+     389             : 
+     390             :     double v_max_actual_vertical, a_max_actual_vertical, j_max_actual_vertical;
+     391           0 :     traj.computeMaxDerivativesVertical(&v_max_actual_vertical, &a_max_actual_vertical, &j_max_actual_vertical);
+     392           0 :     std::cout << "[Time Scaling] Beginning: v: max: " << v_max_actual_vertical << " / " << v_max_vertical << " a: max: " << a_max_actual_vertical << " / "
+     393           0 :               << a_max_vertical << std::endl;
+     394             : 
+     395             :     double v_max_actual_heading, a_max_actual_heading, j_max_actual_heading;
+     396           0 :     traj.computeMaxDerivativesHeading(&v_max_actual_heading, &a_max_actual_heading, &j_max_actual_heading);
+     397           0 :     std::cout << "[Time Scaling] Beginning: v: max: " << v_max_actual_heading << " / " << v_max_heading << " a: max: " << a_max_actual_heading << " / "
+     398           0 :               << a_max_heading << std::endl;
+     399             :   }
+     400             : 
+     401             :   // Run the trajectory time scaling.
+     402          16 :   traj.scaleSegmentTimesToMeetConstraints(v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical, v_max_heading,
+     403             :                                           a_max_heading, j_max_heading);
+     404             : 
+     405          32 :   std::vector<double> segment_times;
+     406          16 :   segment_times = traj.getSegmentTimes();
+     407          16 :   poly_opt_.updateSegmentTimes(segment_times);
+     408          16 :   poly_opt_.solveLinear();
+     409             : 
+     410          16 :   if (optimization_parameters_.print_debug_info_time_allocation) {
+     411             : 
+     412             :     double v_max_actual_horizontal, a_max_actual_horizontal, j_max_actual_horizontal;
+     413           0 :     traj.computeMaxDerivativesHorizontal(&v_max_actual_horizontal, &a_max_actual_horizontal, &j_max_actual_horizontal);
+     414           0 :     std::cout << "[Time Scaling] End: v: max: " << v_max_actual_horizontal << " / " << v_max_horizontal << " a: max: " << a_max_actual_horizontal << " / "
+     415           0 :               << a_max_horizontal << std::endl;
+     416             : 
+     417             :     double v_max_actual_vertical, a_max_actual_vertical, j_max_actual_vertical;
+     418           0 :     traj.computeMaxDerivativesVertical(&v_max_actual_vertical, &a_max_actual_vertical, &j_max_actual_vertical);
+     419           0 :     std::cout << "[Time Scaling] End: v: max: " << v_max_actual_vertical << " / " << v_max_vertical << " a: max: " << a_max_actual_vertical << " / "
+     420           0 :               << a_max_vertical << std::endl;
+     421             : 
+     422             :     double v_max_actual_heading, a_max_actual_heading, j_max_actual_heading;
+     423           0 :     traj.computeMaxDerivativesHeading(&v_max_actual_heading, &a_max_actual_heading, &j_max_actual_heading);
+     424           0 :     std::cout << "[Time Scaling] End: v: max: " << v_max_actual_heading << " / " << v_max_heading << " a: max: " << a_max_actual_heading << " / "
+     425           0 :               << a_max_heading << std::endl;
+     426             :   }
+     427          16 : }
+     428             : 
+     429             : template <int _N>
+     430           0 : int PolynomialOptimizationNonLinear<_N>::optimizeTimeAndFreeConstraints() {
+     431           0 :   std::vector<double> initial_step, initial_solution, segment_times, lower_bounds, upper_bounds;
+     432             : 
+     433           0 :   poly_opt_.getSegmentTimes(&segment_times);
+     434           0 :   const size_t n_segments = segment_times.size();
+     435             : 
+     436             :   // compute initial solution
+     437           0 :   poly_opt_.solveLinear();
+     438           0 :   std::vector<Eigen::VectorXd> free_constraints;
+     439           0 :   poly_opt_.getFreeConstraints(&free_constraints);
+     440           0 :   if (free_constraints.size() == 0 || free_constraints.front().size() == 0) {
+     441           0 :     LOG(WARNING) << "No free derivative variables, same as time-only optimization.";
+     442             :   }
+     443             : 
+     444           0 :   const size_t n_optimization_variables = n_segments + free_constraints.size() * free_constraints.front().size();
+     445             : 
+     446           0 :   CHECK_GT(n_optimization_variables, 0u);
+     447             : 
+     448           0 :   initial_solution.reserve(n_optimization_variables);
+     449           0 :   initial_step.reserve(n_optimization_variables);
+     450           0 :   lower_bounds.reserve(n_optimization_variables);
+     451           0 :   upper_bounds.reserve(n_optimization_variables);
+     452             : 
+     453             :   // copy all constraints into one vector:
+     454           0 :   for (double t : segment_times) {
+     455           0 :     initial_solution.push_back(t);
+     456             :   }
+     457             : 
+     458           0 :   for (const Eigen::VectorXd& c : free_constraints) {
+     459           0 :     for (int i = 0; i < c.size(); ++i) {
+     460           0 :       initial_solution.push_back(c[i]);
+     461             :     }
+     462             :   }
+     463             : 
+     464             :   // Setup for getting bounds on the free endpoint derivatives
+     465           0 :   std::vector<double> lower_bounds_free, upper_bounds_free;
+     466           0 :   const size_t        n_optimization_variables_free = free_constraints.size() * free_constraints.front().size();
+     467           0 :   lower_bounds_free.reserve(n_optimization_variables_free);
+     468           0 :   upper_bounds_free.reserve(n_optimization_variables_free);
+     469             : 
+     470             :   // Get the lower and upper bounds constraints on the free endpoint
+     471             :   // derivatives
+     472           0 :   Vertex::Vector vertices;
+     473           0 :   poly_opt_.getVertices(&vertices);
+     474           0 :   setFreeEndpointDerivativeHardConstraints(vertices, &lower_bounds_free, &upper_bounds_free);
+     475             : 
+     476             :   // Set segment time constraints
+     477           0 :   for (size_t l = 0; l < n_segments; ++l) {
+     478           0 :     lower_bounds.push_back(kOptimizationTimeLowerBound);
+     479           0 :     upper_bounds.push_back(std::numeric_limits<double>::max());
+     480             :   }
+     481             :   // Append free endpoint derivative constraints
+     482           0 :   lower_bounds.insert(std::end(lower_bounds), std::begin(lower_bounds_free), std::end(lower_bounds_free));
+     483           0 :   upper_bounds.insert(std::end(upper_bounds), std::begin(upper_bounds_free), std::end(upper_bounds_free));
+     484             : 
+     485           0 :   for (size_t i = 0; i < initial_solution.size(); i++) {
+     486           0 :     double       x     = initial_solution[i];
+     487           0 :     const double abs_x = std::abs(x);
+     488             :     // Initial step size cannot be 0.0 --> invalid arg
+     489           0 :     if (abs_x <= std::numeric_limits<double>::epsilon()) {
+     490           0 :       initial_step.push_back(1e-13);
+     491             :     } else {
+     492           0 :       initial_step.push_back(optimization_parameters_.initial_stepsize_rel * abs_x);
+     493             :     }
+     494             : 
+     495             :     // Check if initial solution isn't already out of bounds.
+     496           0 :     if (x < lower_bounds[i]) {
+     497           0 :       lower_bounds[i] = x;
+     498           0 :     } else if (x > upper_bounds[i]) {
+     499           0 :       upper_bounds[i] = x;
+     500             :     }
+     501             :   }
+     502             : 
+     503             :   // Make sure everything is the same size, otherwise NLOPT will have a bad
+     504             :   // time.
+     505           0 :   CHECK_EQ(lower_bounds.size(), upper_bounds.size());
+     506           0 :   CHECK_EQ(initial_solution.size(), lower_bounds.size());
+     507           0 :   CHECK_EQ(initial_solution.size(), initial_step.size());
+     508           0 :   CHECK_EQ(initial_solution.size(), n_optimization_variables);
+     509             : 
+     510             :   try {
+     511           0 :     nlopt_->set_initial_step(initial_step);
+     512           0 :     nlopt_->set_lower_bounds(lower_bounds);
+     513           0 :     nlopt_->set_upper_bounds(upper_bounds);
+     514           0 :     nlopt_->set_min_objective(&PolynomialOptimizationNonLinear<N>::objectiveFunctionTimeAndConstraints, this);
+     515             :   }
+     516           0 :   catch (std::exception& e) {
+     517           0 :     LOG(ERROR) << "error while setting up nlopt: " << e.what() << std::endl;
+     518           0 :     return nlopt::FAILURE;
+     519             :   }
+     520             : 
+     521           0 :   double final_cost = std::numeric_limits<double>::max();
+     522             :   int    result;
+     523             : 
+     524             :   try {
+     525           0 :     timing::Timer timer_solve("optimize_nonlinear_full_total_time");
+     526             : 
+     527           0 :     result = nlopt_->optimize(initial_solution, final_cost);
+     528           0 :     timer_solve.Stop();
+     529             :   }
+     530           0 :   catch (std::exception& e) {
+     531           0 :     LOG(ERROR) << "error while running nlopt: " << e.what() << std::endl;
+     532           0 :     return nlopt::FAILURE;
+     533             :   }
+     534             : 
+     535           0 :   return result;
+     536             : }
+     537             : 
+     538             : template <int _N>
+     539         192 : bool PolynomialOptimizationNonLinear<_N>::addMaximumMagnitudeConstraint(int dimension, int derivative, double maximum_value) {
+     540             : 
+     541         192 :   CHECK_GE(derivative, 0);
+     542         192 :   CHECK_GE(maximum_value, 0.0);
+     543             : 
+     544         384 :   std::shared_ptr<ConstraintData> constraint_data(new ConstraintData);
+     545         192 :   constraint_data->dimension   = dimension;
+     546         192 :   constraint_data->derivative  = derivative;
+     547         192 :   constraint_data->value       = maximum_value;
+     548         192 :   constraint_data->this_object = this;
+     549             : 
+     550             :   // Store the shared_ptrs such that their data will be destroyed later.
+     551         192 :   inequality_constraints_.push_back(constraint_data);
+     552             : 
+     553         192 :   if (!optimization_parameters_.use_soft_constraints) {
+     554             :     try {
+     555           0 :       nlopt_->add_inequality_constraint(&PolynomialOptimizationNonLinear<N>::evaluateMaximumMagnitudeConstraint, constraint_data.get(),
+     556             :                                         optimization_parameters_.inequality_constraint_tolerance);
+     557             :     }
+     558           0 :     catch (std::exception& e) {
+     559           0 :       LOG(ERROR) << "ERROR while setting inequality constraint " << e.what() << std::endl;
+     560           0 :       return false;
+     561             :     }
+     562             :   }
+     563             : 
+     564         192 :   return true;
+     565             : }
+     566             : 
+     567             : template <int _N>
+     568           0 : double PolynomialOptimizationNonLinear<_N>::objectiveFunctionTime(const std::vector<double>& segment_times, std::vector<double>& gradient, void* data) {
+     569           0 :   CHECK(gradient.empty()) << "computing gradient not possible, choose a gradient free method";
+     570           0 :   CHECK_NOTNULL(data);
+     571             : 
+     572           0 :   PolynomialOptimizationNonLinear<N>* optimization_data = static_cast<PolynomialOptimizationNonLinear<N>*>(data);  // wheee ...
+     573             : 
+     574           0 :   CHECK_EQ(segment_times.size(), optimization_data->poly_opt_.getNumberSegments());
+     575             : 
+     576           0 :   optimization_data->poly_opt_.updateSegmentTimes(segment_times);
+     577           0 :   optimization_data->poly_opt_.solveLinear();
+     578           0 :   double       cost_trajectory  = optimization_data->poly_opt_.computeCost();
+     579           0 :   double       cost_time        = 0;
+     580           0 :   double       cost_constraints = 0;
+     581           0 :   const double total_time       = computeTotalTrajectoryTime(segment_times);
+     582             : 
+     583           0 :   switch (optimization_data->optimization_parameters_.time_alloc_method) {
+     584           0 :     case NonlinearOptimizationParameters::kRichterTime:
+     585           0 :       cost_time = total_time * optimization_data->optimization_parameters_.time_penalty;
+     586           0 :       break;
+     587           0 :     default:  // kSquaredTime
+     588           0 :       cost_time = total_time * total_time * optimization_data->optimization_parameters_.time_penalty;
+     589           0 :       break;
+     590             :   }
+     591             : 
+     592           0 :   if (optimization_data->optimization_parameters_.print_debug_info) {
+     593           0 :     std::cout << "---- cost at iteration " << optimization_data->optimization_info_.n_iterations << "---- " << std::endl;
+     594           0 :     std::cout << "  trajectory: " << cost_trajectory << std::endl;
+     595           0 :     std::cout << "  time: " << cost_time << std::endl;
+     596             :   }
+     597             : 
+     598           0 :   if (optimization_data->optimization_parameters_.use_soft_constraints) {
+     599           0 :     cost_constraints = optimization_data->evaluateMaximumMagnitudeAsSoftConstraint(optimization_data->inequality_constraints_,
+     600             :                                                                                    optimization_data->optimization_parameters_.soft_constraint_weight);
+     601             :   }
+     602             : 
+     603           0 :   if (optimization_data->optimization_parameters_.print_debug_info) {
+     604           0 :     std::cout << "  sum: " << cost_trajectory + cost_time + cost_constraints << std::endl;
+     605           0 :     std::cout << "  total time: " << total_time << std::endl;
+     606             :   }
+     607             : 
+     608           0 :   optimization_data->optimization_info_.n_iterations++;
+     609           0 :   optimization_data->optimization_info_.cost_trajectory       = cost_trajectory;
+     610           0 :   optimization_data->optimization_info_.cost_time             = cost_time;
+     611           0 :   optimization_data->optimization_info_.cost_soft_constraints = cost_constraints;
+     612             : 
+     613           0 :   return cost_trajectory + cost_time + cost_constraints;
+     614             : }
+     615             : 
+     616             : template <int _N>
+     617         211 : double PolynomialOptimizationNonLinear<_N>::objectiveFunctionTimeMellingerOuterLoop(const std::vector<double>& segment_times, std::vector<double>& gradient,
+     618             :                                                                                     void* data) {
+     619         211 :   CHECK(!gradient.empty()) << "only with gradients possible, choose a gradient based method";
+     620         211 :   CHECK_NOTNULL(data);
+     621             : 
+     622         211 :   PolynomialOptimizationNonLinear<N>* optimization_data = static_cast<PolynomialOptimizationNonLinear<N>*>(data);  // wheee ...
+     623             : 
+     624         211 :   CHECK_EQ(segment_times.size(), optimization_data->poly_opt_.getNumberSegments());
+     625             : 
+     626         211 :   optimization_data->poly_opt_.updateSegmentTimes(segment_times);
+     627         211 :   optimization_data->poly_opt_.solveLinear();
+     628             :   double cost_trajectory;
+     629         211 :   if (!gradient.empty()) {
+     630         211 :     cost_trajectory = optimization_data->getCostAndGradientMellinger(&gradient);
+     631             :   } else {
+     632           0 :     cost_trajectory = optimization_data->getCostAndGradientMellinger(NULL);
+     633             :   }
+     634             : 
+     635         211 :   if (optimization_data->optimization_parameters_.print_debug_info) {
+     636           0 :     std::cout << "---- cost at iteration " << optimization_data->optimization_info_.n_iterations << "---- " << std::endl;
+     637           0 :     std::cout << "  segment times: ";
+     638           0 :     for (double segment_time : segment_times) {
+     639           0 :       std::cout << segment_time << " ";
+     640             :     }
+     641           0 :     std::cout << std::endl;
+     642           0 :     std::cout << "  sum: " << cost_trajectory << std::endl;
+     643             :   }
+     644             : 
+     645         211 :   optimization_data->optimization_info_.n_iterations++;
+     646         211 :   optimization_data->optimization_info_.cost_trajectory = cost_trajectory;
+     647             : 
+     648         211 :   return cost_trajectory;
+     649             : }
+     650             : 
+     651             : template <int _N>
+     652           0 : double PolynomialOptimizationNonLinear<_N>::objectiveFunctionTimeAndConstraints(const std::vector<double>& x, std::vector<double>& gradient, void* data) {
+     653           0 :   CHECK(gradient.empty()) << "computing gradient not possible, choose a gradient-free method";
+     654           0 :   CHECK_NOTNULL(data);
+     655             : 
+     656           0 :   PolynomialOptimizationNonLinear<N>* optimization_data = static_cast<PolynomialOptimizationNonLinear<N>*>(data);  // wheee ...
+     657             : 
+     658           0 :   const size_t n_segments         = optimization_data->poly_opt_.getNumberSegments();
+     659           0 :   const size_t n_free_constraints = optimization_data->poly_opt_.getNumberFreeConstraints();
+     660           0 :   const size_t dim                = optimization_data->poly_opt_.getDimension();
+     661             : 
+     662           0 :   CHECK_EQ(x.size(), n_segments + n_free_constraints * dim);
+     663             : 
+     664           0 :   std::vector<Eigen::VectorXd> free_constraints;
+     665           0 :   free_constraints.resize(dim);
+     666           0 :   std::vector<double> segment_times;
+     667           0 :   segment_times.reserve(n_segments);
+     668             : 
+     669           0 :   for (size_t i = 0; i < n_segments; ++i) {
+     670           0 :     segment_times.push_back(x[i]);
+     671             :   }
+     672             : 
+     673           0 :   for (size_t d = 0; d < dim; ++d) {
+     674           0 :     const size_t idx_start = n_segments + d * n_free_constraints;
+     675             : 
+     676           0 :     Eigen::VectorXd& free_constraints_dim = free_constraints[d];
+     677           0 :     free_constraints_dim.resize(n_free_constraints, Eigen::NoChange);
+     678           0 :     for (size_t i = 0; i < n_free_constraints; ++i) {
+     679           0 :       free_constraints_dim[i] = x[idx_start + i];
+     680             :     }
+     681             :   }
+     682             : 
+     683           0 :   optimization_data->poly_opt_.updateSegmentTimes(segment_times);
+     684           0 :   optimization_data->poly_opt_.setFreeConstraints(free_constraints);
+     685             : 
+     686           0 :   double cost_trajectory  = optimization_data->poly_opt_.computeCost();
+     687           0 :   double cost_time        = 0;
+     688           0 :   double cost_constraints = 0;
+     689             : 
+     690           0 :   const double total_time = computeTotalTrajectoryTime(segment_times);
+     691           0 :   switch (optimization_data->optimization_parameters_.time_alloc_method) {
+     692           0 :     case NonlinearOptimizationParameters::kRichterTimeAndConstraints:
+     693           0 :       cost_time = total_time * optimization_data->optimization_parameters_.time_penalty;
+     694           0 :       break;
+     695           0 :     default:  // kSquaredTimeAndConstraints
+     696           0 :       cost_time = total_time * total_time * optimization_data->optimization_parameters_.time_penalty;
+     697           0 :       break;
+     698             :   }
+     699             : 
+     700           0 :   if (optimization_data->optimization_parameters_.print_debug_info) {
+     701           0 :     std::cout << "---- cost at iteration " << optimization_data->optimization_info_.n_iterations << "---- " << std::endl;
+     702           0 :     std::cout << "  trajectory: " << cost_trajectory << std::endl;
+     703           0 :     std::cout << "  time: " << cost_time << std::endl;
+     704             :   }
+     705             : 
+     706           0 :   if (optimization_data->optimization_parameters_.use_soft_constraints) {
+     707           0 :     cost_constraints = optimization_data->evaluateMaximumMagnitudeAsSoftConstraint(optimization_data->inequality_constraints_,
+     708             :                                                                                    optimization_data->optimization_parameters_.soft_constraint_weight);
+     709             :   }
+     710             : 
+     711           0 :   if (optimization_data->optimization_parameters_.print_debug_info) {
+     712           0 :     std::cout << "  sum: " << cost_trajectory + cost_time + cost_constraints << std::endl;
+     713           0 :     std::cout << "  total time: " << total_time << std::endl;
+     714             :   }
+     715             : 
+     716           0 :   optimization_data->optimization_info_.n_iterations++;
+     717           0 :   optimization_data->optimization_info_.cost_trajectory       = cost_trajectory;
+     718           0 :   optimization_data->optimization_info_.cost_time             = cost_time;
+     719           0 :   optimization_data->optimization_info_.cost_soft_constraints = cost_constraints;
+     720             : 
+     721           0 :   return cost_trajectory + cost_time + cost_constraints;
+     722             : }
+     723             : 
+     724             : template <int _N>
+     725           0 : double PolynomialOptimizationNonLinear<_N>::evaluateMaximumMagnitudeConstraint([[maybe_unused]] const std::vector<double>& segment_times,
+     726             :                                                                                std::vector<double>& gradient, void* data) {
+     727           0 :   CHECK(gradient.empty()) << "computing gradient not possible, choose a gradient-free method";
+     728           0 :   ConstraintData*                     constraint_data   = static_cast<ConstraintData*>(data);  // wheee ...
+     729           0 :   PolynomialOptimizationNonLinear<N>* optimization_data = constraint_data->this_object;
+     730             : 
+     731           0 :   Extremum max;
+     732           0 :   max = optimization_data->poly_opt_.computeMaximumOfMagnitude(constraint_data->derivative, nullptr);
+     733             : 
+     734           0 :   optimization_data->optimization_info_.maxima[constraint_data->derivative] = max;
+     735             : 
+     736           0 :   return max.value - constraint_data->value;
+     737             : }
+     738             : 
+     739             : template <int _N>
+     740           0 : double PolynomialOptimizationNonLinear<_N>::evaluateMaximumMagnitudeAsSoftConstraint(
+     741             :     [[maybe_unused]] const std::vector<std::shared_ptr<ConstraintData>>& inequality_constraints, double weight, double maximum_cost) const {
+     742           0 :   std::vector<double> dummy;
+     743           0 :   double              cost = 0;
+     744             : 
+     745           0 :   if (optimization_parameters_.print_debug_info)
+     746           0 :     std::cout << "  soft_constraints: " << std::endl;
+     747             : 
+     748           0 :   for (std::shared_ptr<const ConstraintData> constraint : inequality_constraints_) {
+     749             :     // need to call the c-style callback function here, thus the ugly cast to
+     750             :     // void*.
+     751           0 :     double abs_violation = evaluateMaximumMagnitudeConstraint(dummy, dummy, (void*)constraint.get());
+     752             : 
+     753           0 :     double       relative_violation = abs_violation / constraint->value;
+     754           0 :     const double current_cost       = std::min(maximum_cost, exp(relative_violation * weight));
+     755           0 :     cost += current_cost;
+     756           0 :     if (optimization_parameters_.print_debug_info) {
+     757           0 :       std::cout << "    derivative " << constraint->derivative << " abs violation: " << abs_violation << " : relative violation: " << relative_violation
+     758           0 :                 << " cost: " << current_cost << std::endl;
+     759             :     }
+     760             :   }
+     761           0 :   return cost;
+     762             : }
+     763             : 
+     764             : template <int _N>
+     765           0 : void PolynomialOptimizationNonLinear<_N>::setFreeEndpointDerivativeHardConstraints(const Vertex::Vector& vertices, std::vector<double>* lower_bounds,
+     766             :                                                                                    std::vector<double>* upper_bounds) {
+     767           0 :   CHECK_NOTNULL(lower_bounds);
+     768           0 :   CHECK_NOTNULL(upper_bounds);
+     769           0 :   CHECK(lower_bounds->empty()) << "Lower bounds not empty!";
+     770           0 :   CHECK(upper_bounds->empty()) << "Upper bounds not empty!";
+     771             : 
+     772           0 :   const size_t n_free_constraints     = poly_opt_.getNumberFreeConstraints();
+     773           0 :   const size_t dim                    = poly_opt_.getDimension();
+     774           0 :   const int    derivative_to_optimize = poly_opt_.getDerivativeToOptimize();
+     775             : 
+     776             :   // Set all values to -inf/inf and reset only bounded opti param with values
+     777           0 :   lower_bounds->resize(dim * n_free_constraints, std::numeric_limits<double>::lowest());
+     778           0 :   upper_bounds->resize(dim * n_free_constraints, std::numeric_limits<double>::max());
+     779             : 
+     780             :   // Add higher order derivative constraints (v_max and a_max)
+     781             :   // Check at each vertex which of the derivatives is a free derivative.
+     782             :   // If it is a free derivative check if we have a constraint in
+     783             :   // inequality_constraints_ and set the constraint as hard constraint in
+     784             :   // lower_bounds and upper_bounds
+     785           0 :   for (const auto& constraint_data : inequality_constraints_) {
+     786           0 :     unsigned int free_deriv_counter = 0;
+     787           0 :     const int    derivative_hc      = constraint_data->derivative;
+     788           0 :     const int    dimension          = constraint_data->dimension;
+     789           0 :     const double value_hc           = constraint_data->value;
+     790             : 
+     791           0 :     for (size_t v = 0; v < vertices.size(); ++v) {
+     792           0 :       for (int deriv = 0; deriv <= derivative_to_optimize; ++deriv) {
+     793           0 :         if (!vertices[v].hasConstraint(deriv)) {
+     794           0 :           if (deriv == derivative_hc) {
+     795           0 :             unsigned int start_idx                           = dimension * n_free_constraints;
+     796           0 :             lower_bounds->at(start_idx + free_deriv_counter) = -std::abs(value_hc);
+     797           0 :             upper_bounds->at(start_idx + free_deriv_counter) = std::abs(value_hc);
+     798             :           }
+     799           0 :           free_deriv_counter++;
+     800             :         }
+     801             :       }
+     802             :     }
+     803             :   }
+     804           0 : }
+     805             : 
+     806             : template <int _N>
+     807           0 : double PolynomialOptimizationNonLinear<_N>::computeTotalTrajectoryTime(const std::vector<double>& segment_times) {
+     808           0 :   double total_time = 0;
+     809           0 :   for (double t : segment_times)
+     810           0 :     total_time += t;
+     811           0 :   return total_time;
+     812             : }
+     813             : 
+     814             : }  // namespace eth_trajectory_generation
+     815             : 
+     816             : namespace nlopt
+     817             : {
+     818             : 
+     819             : inline std::string returnValueToString(int return_value) {
+     820             :   switch (return_value) {
+     821             :     case nlopt::SUCCESS:
+     822             :       return std::string("SUCCESS");
+     823             :     case nlopt::FAILURE:
+     824             :       return std::string("FAILURE");
+     825             :     case nlopt::INVALID_ARGS:
+     826             :       return std::string("INVALID_ARGS");
+     827             :     case nlopt::OUT_OF_MEMORY:
+     828             :       return std::string("OUT_OF_MEMORY");
+     829             :     case nlopt::ROUNDOFF_LIMITED:
+     830             :       return std::string("ROUNDOFF_LIMITED");
+     831             :     case nlopt::FORCED_STOP:
+     832             :       return std::string("FORCED_STOP");
+     833             :     case nlopt::STOPVAL_REACHED:
+     834             :       return std::string("STOPVAL_REACHED");
+     835             :     case nlopt::FTOL_REACHED:
+     836             :       return std::string("FTOL_REACHED");
+     837             :     case nlopt::XTOL_REACHED:
+     838             :       return std::string("XTOL_REACHED");
+     839             :     case nlopt::MAXEVAL_REACHED:
+     840             :       return std::string("MAXEVAL_REACHED");
+     841             :     case nlopt::MAXTIME_REACHED:
+     842             :       return std::string("MAXTIME_REACHED");
+     843             :     default:
+     844             :       return std::string("ERROR CODE UNKNOWN");
+     845             :   }
+     846             : }
+     847             : }  // namespace nlopt
+     848             : 
+     849             : #endif  // ETH_TRAJECTORY_GENERATION_IMPL_POLYNOMIAL_OPTIMIZATION_NONLINEAR_IMPL_H_
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.overview.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.overview.html new file mode 100644 index 0000000000..4e59135724 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.overview.html @@ -0,0 +1,233 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.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 + + +
    + Top

    + Overview +
    + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..113232edd09b069a37fc4fc2b06d2ef71f92fcc7 GIT binary patch literal 3493 zcmV;W4O;SvP)7j-N;6O*oPqV|`6TX1&?%E~a-U3* zoXEP*Jb9S}U=Kxa=F$Ij0#D%;SYT$XN34y2pMqu+tC<-Lg1=IIPG}-lg~)BhAHMn| z)`GqfsD5&_fK;tw2llY8YnuZ*Xj>_^6p?|S)WVz_QX_VIRS!uQs#5UTB$I2wb8!C= z03*7(KysQ8cYZuk;;{{7z+l=0WV91-itkNQRVCnrR;0khnF6Eu2qY*Dj2dOE$BDB( ze{l+h#UawpVBitjx@gD&=+WXL&_o*mV3azEI(B+R6ZeQehOVpy{IQhv=Vy4mR`y0- zmRBgl`m&Y5hH@qClMWDKLQ6$QcP%+QCj}4BsqRz@pe6m*cz6f+e zR$@4j(UGKr>8u^lV4iJzpJi*76X=mpjw3zqJNY02EfkM7$OFv4x7az9Pp)UlgVgT#`!}#O4{Ej*RGwZpYpdry_(We=@$i)mxoVanz@Da% zaa<(B_BXC3@QX40edNBK+5oNHf%KW|SMDc+%_SJcTGI+Z$7~Nm<&E?f)`}yaz{4{I zuDeQ6)`pccjy%z3hP=0}Ajf$O3uU-kNEX|s)HmB^q_Wn6f=%-rCSb3@U9lT{OFsSH z5B*!fdXG-Iwazu8(c6i`U88?2mHix(hP?FIDJm<(`gc zx$V!W!X`x{=ML7NkD5twBT3S`!Z#|NX1#9*pt;kW$F%T2R>E2-BaQ-o=%n<+DJi85 zz~GPIe$Fv9H`|~u46!D)IF~^Jo_TFJqbqS^}h>D_8=g1De$4GFAd410Fc) zD8HBkBeG=BQ8N#$u;E_)RNa`Bd)Ly_D=Wk5N?f)5OM?#?tg~Kops@t=jF@wv_c>RF z3cgNbM3>y`EthP>-O;fpAPiK#!1|ijobz#!b!@1lH8N#_X#4IB_5qYNiVYu&RXw>_USfHQrLq~Yhd#)MhPa=9#L4H}2&v&PqJl$f)|lwthI z`3KgiZKDH$31W2fz}!Bh1{Nyg%Slz6gd_09sK~qg9=O(9L5t!~zJl~JEi5<-Ju~4O zQ$}h?(3V^yuE344Bd$?|dQQK^h1$T^C1YlJ>Qk1`IS-I^-eNuQgwJtQ)_`21KN$fe z&H_c99aqMNvzJse(Uc+nl=@^tQia?Lk{nrsDg8M3qp%aO7BhC6-*!wYqB3ci0*Zqt zfr78fvTL4iMnBVKv?v0~q(C~+he?QLN@tj}Mj)<#I((LCbVY{ENqg3V*>6Z0G|f_$-UYTw7CtwRI@_TX zbDz-6bwS3~q>~P2)?!4K$#^r+stlo6BlrmLf{fy5`AMK%MzV#I1^a~o+2mCpnZoco zQQ=)Oel3h~0fgU>=L%~^XDwRrax29p0qqoR1}`|fVg5~atciHdjvrDK?AWIGbvu$~ zAhYha}a2x-9CpiZ#G#JiOQtWHq6M#tSU`4nt6LWZj#_S+=1`Am)T}X z+D(DgIAu%6o*tSjGl{C4BaVhLD5h1JJy%BEp1Q7#{5UAdoTV;SWsag|FukNy=7Ef% zapt=pN+YT!JSx?cX#i{Oy4COr#oriGgm|(a$1e>j zLOeN2WqwIW5#q_d7xD3ikn+Wo$d$tvN$sO{_yQkxu%l7yFCM;t1&!wV%vKjoRc6<| zqi9_shmD1VL&)Pee5NIO`1$#PJz)?cIxp1B6Zzp-Gayvt&EnZ7P9bj|e(GjkR2#5# zv-^QoWrnnXo3e`Knz?#vNCiINJ=A%Uta+wn%s4+|Z^RsmKGH_FDD~H~74fpC33f~7 zVtKBJxWrgI^)eu~LVI@b#&yGrKS~shKx0XRVRr2mRiBziaq7%_h)HU!J5JhZWoJ*l zTBlt_)tORr-DBk3%L5f#60%1kAStl+!JD4z$H)7tN}fLKTXV4Bp^(Cj!N|hg(P^ zjK9W-HwbHi10e*N6=yX>n{L4zC3qg?ZGf%QcGMJhE&_K~NWb#f2fV%2{7b$Wl(Atmzzt?8jE?2$)2)sXIrwI3z3veuWm%lq~k8Ke=o8G9r#nu$R3z-KcCACxf* zKDgEe9auU`+Gyb49K^*`j&{v_^ID$P1y#}XY!G*lI@xjY@hE_;nz0pO0!h}8T8?X? zsF~o7^~We^9Vksng8{LBl?NdQ2fA-+lbW2OI{V>E@t&eodaJPIXZse%6bewf)lFQ8 zW~J{|w-flT&Hq)_!YKBucYSUxb06f2Iy`C%I|;&J(gfV%_mKnN8)HB~N!w@N&^qc( zhS$31M6Ind-rjxmSK&shE-s>zZkN|gsvO}yfJX-Gni`B^1BmLhwq4NS8lm|h6s>hl zquIWvpOIq%es%U?J*R0Me6TTYJWd4%JISqy3k5bPUSb^v!D%%7xiS@7eIjEfaUH1K zPqC;UGhh;-&GO0*Zh)?R~$!nC{A1>{57go>K_9D~smym<>wdJr6mq=*ot{GA%>Ou+wx>uFm zVMejGjeIdaYAY@|Ypt}_QmE3;uca_T$6ftONMTLJ618RKiO+BS z3h%z*j4YTyG03;0u!p$1(K}6{hQQx>Q-Je7X*nk(xrFr2Fn>7wIi%5aZy`;Ik$Xd* z6x!ps*{&U`KvY3snm6a$c1e~nECeDA)^Z*01rX~nZr`&(k4yB7(JU5k8w8aDng^yS zv0~a3zZw8c?f-fpy&njP8nK2i0Xvl83q#_H&q09c%Lj*0ipNH*F7;$RGU5pc_^Z=+ za}eh6hDgH9!{+w9bO8HgOn(X((wk=X$FIHT#$;|lV=B~?{V8$(SGK2=u^jJeix;QP zoFX~HNzP8=ooo{Y!Z9c{#~F_o?WKw6l6>rEcDAl4yBYV2+l0vNy9M~|W8!O5L+I4ztrr7}AKLVIi zx+s><);J){wpo9~Mq!OT1_|ZimyB#C+$UmY?hW{p6kB%BD5~)M7K09o3fy>2y+~1p zvkNf#WK?k`bIUR|Dc)Z*57hPTTHi&{dRslEh@-v|cKf|kDUjY3*PP0Qb%gRCAWFg@ ToR~S400000NkvXXu0mjf{Cll~ literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail-sort-f.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail-sort-f.html new file mode 100644 index 0000000000..b4a9e87b13 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail-sort-f.html @@ -0,0 +1,228 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generationHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:12920562.9 %
    Date:2024-04-09 22:03:33Functions:376061.7 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
    timing.h +
    0.0%
    +     		0.0 %0 / 390.0 %0 / 10
    polynomial_optimization_linear.h +
    65.0%65.0%
    +     		65.0 %26 / 4054.5 %6 / 11
    <unnamed>65.0 %26 / 4054.5 %6 / 11
    trajectory.h +
    61.0%61.0%
    +     		61.0 %25 / 4158.3 %7 / 12
    <unnamed>61.0 %25 / 4158.3 %7 / 12
    vertex.h +
    75.0%75.0%
    +     		75.0 %6 / 875.0 %3 / 4
    <unnamed>75.0 %6 / 875.0 %3 / 4
    polynomial.h +
    89.8%89.8%
    +     		89.8 %44 / 4980.0 %8 / 10
    <unnamed>89.8 %44 / 4980.0 %8 / 10
    polynomial_optimization_nonlinear.h +
    100.0%
    +     		100.0 %7 / 7100.0 %3 / 3
    <unnamed>100.0 %7 / 7100.0 %3 / 3
    extremum.h +
    100.0%
    +     		100.0 %8 / 8100.0 %4 / 4
    <unnamed>100.0 %8 / 8100.0 %4 / 4
    segment.h +
    100.0%
    +     		100.0 %13 / 13100.0 %6 / 6
    <unnamed>100.0 %13 / 13100.0 %6 / 6
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail-sort-l.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail-sort-l.html new file mode 100644 index 0000000000..f1a958b78e --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail-sort-l.html @@ -0,0 +1,228 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generationHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:12920562.9 %
    Date:2024-04-09 22:03:33Functions:376061.7 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
    timing.h +
    0.0%
    +     		0.0 %0 / 390.0 %0 / 10
    trajectory.h +
    61.0%61.0%
    +     		61.0 %25 / 4158.3 %7 / 12
    <unnamed>61.0 %25 / 4158.3 %7 / 12
    polynomial_optimization_linear.h +
    65.0%65.0%
    +     		65.0 %26 / 4054.5 %6 / 11
    <unnamed>65.0 %26 / 4054.5 %6 / 11
    vertex.h +
    75.0%75.0%
    +     		75.0 %6 / 875.0 %3 / 4
    <unnamed>75.0 %6 / 875.0 %3 / 4
    polynomial.h +
    89.8%89.8%
    +     		89.8 %44 / 4980.0 %8 / 10
    <unnamed>89.8 %44 / 4980.0 %8 / 10
    polynomial_optimization_nonlinear.h +
    100.0%
    +     		100.0 %7 / 7100.0 %3 / 3
    <unnamed>100.0 %7 / 7100.0 %3 / 3
    extremum.h +
    100.0%
    +     		100.0 %8 / 8100.0 %4 / 4
    <unnamed>100.0 %8 / 8100.0 %4 / 4
    segment.h +
    100.0%
    +     		100.0 %13 / 13100.0 %6 / 6
    <unnamed>100.0 %13 / 13100.0 %6 / 6
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail.html new file mode 100644 index 0000000000..d3b88eacf5 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-detail.html @@ -0,0 +1,228 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generationHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:12920562.9 %
    Date:2024-04-09 22:03:33Functions:376061.7 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
    extremum.h +
    100.0%
    +     		100.0 %8 / 8100.0 %4 / 4
    <unnamed>100.0 %8 / 8100.0 %4 / 4
    polynomial.h +
    89.8%89.8%
    +     		89.8 %44 / 4980.0 %8 / 10
    <unnamed>89.8 %44 / 4980.0 %8 / 10
    polynomial_optimization_linear.h +
    65.0%65.0%
    +     		65.0 %26 / 4054.5 %6 / 11
    <unnamed>65.0 %26 / 4054.5 %6 / 11
    polynomial_optimization_nonlinear.h +
    100.0%
    +     		100.0 %7 / 7100.0 %3 / 3
    <unnamed>100.0 %7 / 7100.0 %3 / 3
    segment.h +
    100.0%
    +     		100.0 %13 / 13100.0 %6 / 6
    <unnamed>100.0 %13 / 13100.0 %6 / 6
    timing.h +
    0.0%
    +     		0.0 %0 / 390.0 %0 / 10
    trajectory.h +
    61.0%61.0%
    +     		61.0 %25 / 4158.3 %7 / 12
    <unnamed>61.0 %25 / 4158.3 %7 / 12
    vertex.h +
    75.0%75.0%
    +     		75.0 %6 / 875.0 %3 / 4
    <unnamed>75.0 %6 / 875.0 %3 / 4
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-sort-f.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-sort-f.html new file mode 100644 index 0000000000..f19a57e749 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-sort-f.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generationHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:12920562.9 %
    Date:2024-04-09 22:03:33Functions:376061.7 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
    timing.h +
    0.0%
    +     		0.0 %0 / 390.0 %0 / 10
    polynomial_optimization_linear.h +
    65.0%65.0%
    +     		65.0 %26 / 4054.5 %6 / 11
    trajectory.h +
    61.0%61.0%
    +     		61.0 %25 / 4158.3 %7 / 12
    vertex.h +
    75.0%75.0%
    +     		75.0 %6 / 875.0 %3 / 4
    polynomial.h +
    89.8%89.8%
    +     		89.8 %44 / 4980.0 %8 / 10
    polynomial_optimization_nonlinear.h +
    100.0%
    +     		100.0 %7 / 7100.0 %3 / 3
    extremum.h +
    100.0%
    +     		100.0 %8 / 8100.0 %4 / 4
    segment.h +
    100.0%
    +     		100.0 %13 / 13100.0 %6 / 6
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-sort-l.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-sort-l.html new file mode 100644 index 0000000000..3c30703339 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index-sort-l.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generationHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:12920562.9 %
    Date:2024-04-09 22:03:33Functions:376061.7 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
    timing.h +
    0.0%
    +     		0.0 %0 / 390.0 %0 / 10
    trajectory.h +
    61.0%61.0%
    +     		61.0 %25 / 4158.3 %7 / 12
    polynomial_optimization_linear.h +
    65.0%65.0%
    +     		65.0 %26 / 4054.5 %6 / 11
    vertex.h +
    75.0%75.0%
    +     		75.0 %6 / 875.0 %3 / 4
    polynomial.h +
    89.8%89.8%
    +     		89.8 %44 / 4980.0 %8 / 10
    polynomial_optimization_nonlinear.h +
    100.0%
    +     		100.0 %7 / 7100.0 %3 / 3
    extremum.h +
    100.0%
    +     		100.0 %8 / 8100.0 %4 / 4
    segment.h +
    100.0%
    +     		100.0 %13 / 13100.0 %6 / 6
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index.html new file mode 100644 index 0000000000..84165bd6e1 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/index.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generationHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:12920562.9 %
    Date:2024-04-09 22:03:33Functions:376061.7 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
    extremum.h +
    100.0%
    +     		100.0 %8 / 8100.0 %4 / 4
    polynomial.h +
    89.8%89.8%
    +     		89.8 %44 / 4980.0 %8 / 10
    polynomial_optimization_linear.h +
    65.0%65.0%
    +     		65.0 %26 / 4054.5 %6 / 11
    polynomial_optimization_nonlinear.h +
    100.0%
    +     		100.0 %7 / 7100.0 %3 / 3
    segment.h +
    100.0%
    +     		100.0 %13 / 13100.0 %6 / 6
    timing.h +
    0.0%
    +     		0.0 %0 / 390.0 %0 / 10
    trajectory.h +
    61.0%61.0%
    +     		61.0 %25 / 4158.3 %7 / 12
    vertex.h +
    75.0%75.0%
    +     		75.0 %6 / 875.0 %3 / 4
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func-sort-c.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func-sort-c.html new file mode 100644 index 0000000000..ef470deb8c --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:444989.8 %
    Date:2024-04-09 22:03:33Functions:81080.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::Polynomial::operator==(eth_trajectory_generation::Polynomial const&) const0
    eth_trajectory_generation::Polynomial::operator!=(eth_trajectory_generation::Polynomial const&) const0
    eth_trajectory_generation::Polynomial::Polynomial(int)16
    eth_trajectory_generation::Polynomial::Polynomial(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)990
    eth_trajectory_generation::Polynomial::getConvolutionLength(int, int)2970
    eth_trajectory_generation::Polynomial::evaluate(double, int) const46492
    eth_trajectory_generation::Polynomial::Polynomial(int, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)74944
    eth_trajectory_generation::Polynomial::getCoefficients(int) const75234
    eth_trajectory_generation::Polynomial::baseCoeffsWithTime(int, int, double)189010
    eth_trajectory_generation::Polynomial::baseCoeffsWithTime(int, int, double, Eigen::Matrix<double, -1, 1, 0, -1, 1>*)189010
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func.html new file mode 100644 index 0000000000..9437b239fb --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:444989.8 %
    Date:2024-04-09 22:03:33Functions:81080.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::Polynomial::baseCoeffsWithTime(int, int, double)189010
    eth_trajectory_generation::Polynomial::baseCoeffsWithTime(int, int, double, Eigen::Matrix<double, -1, 1, 0, -1, 1>*)189010
    eth_trajectory_generation::Polynomial::getConvolutionLength(int, int)2970
    eth_trajectory_generation::Polynomial::Polynomial(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)990
    eth_trajectory_generation::Polynomial::Polynomial(int)16
    eth_trajectory_generation::Polynomial::Polynomial(int, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)74944
    eth_trajectory_generation::Polynomial::getCoefficients(int) const75234
    eth_trajectory_generation::Polynomial::evaluate(double, int) const46492
    eth_trajectory_generation::Polynomial::operator==(eth_trajectory_generation::Polynomial const&) const0
    eth_trajectory_generation::Polynomial::operator!=(eth_trajectory_generation::Polynomial const&) const0
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.frameset.html new file mode 100644 index 0000000000..1c7c9ff042 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.html new file mode 100644 index 0000000000..1576629c2f --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.html @@ -0,0 +1,353 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:444989.8 %
    Date:2024-04-09 22:03:33Functions:81080.0 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #ifndef ETH_TRAJECTORY_GENERATION_POLYNOMIAL_H_
+      22             : #define ETH_TRAJECTORY_GENERATION_POLYNOMIAL_H_
+      23             : 
+      24             : #include <eth_trajectory_generation/misc.h>
+      25             : #include <Eigen/Eigen>
+      26             : #include <Eigen/SVD>
+      27             : #include <utility>
+      28             : #include <vector>
+      29             : 
+      30             : namespace eth_trajectory_generation
+      31             : {
+      32             : 
+      33             : // Implementation of polynomials of order N-1. Order must be known at
+      34             : // compile time.
+      35             : // Polynomial coefficients are stored with increasing powers,
+      36             : // i.e. c_0 + c_1*t ... c_{N-1} * t^{N-1}
+      37             : // where N = number of coefficients of the polynomial.
+      38             : class Polynomial {
+      39             : public:
+      40             :   typedef std::vector<Polynomial> Vector;
+      41             : 
+      42             :   // Maximum degree of a polynomial for which the static derivative (basis
+      43             :   // coefficient) matrix should be evaluated for.
+      44             :   // kMaxN = max. number of coefficients.
+      45             :   static constexpr int kMaxN = 12;
+      46             :   // kMaxConvolutionSize = max. convolution size for N = 12, convolved with its
+      47             :   // derivative.
+      48             :   static constexpr int kMaxConvolutionSize = 2 * kMaxN - 2;
+      49             :   // One static shared across all members of the class, computed up to order
+      50             :   // kMaxConvolutionSize.
+      51             :   static Eigen::MatrixXd base_coefficients_;
+      52             : 
+      53          16 :   Polynomial(int N) : N_(N), coefficients_(N) {
+      54          16 :     coefficients_.setZero();
+      55          16 :   }
+      56             : 
+      57             :   // Assigns arbitrary coefficients to a polynomial.
+      58       74944 :   Polynomial(int N, const Eigen::VectorXd& coeffs) : N_(N), coefficients_(coeffs) {
+      59       74944 :     CHECK_EQ(N_, coeffs.size()) << "Number of coefficients has to match.";
+      60       74944 :   }
+      61             : 
+      62         990 :   Polynomial(const Eigen::VectorXd& coeffs) : N_(coeffs.size()), coefficients_(coeffs) {
+      63         990 :   }
+      64             :   /// Gets the number of coefficients (order + 1) of the polynomial.
+      65             :   int N() const {
+      66             :     return N_;
+      67             :   }
+      68             : 
+      69           0 :   inline bool operator==(const Polynomial& rhs) const {
+      70           0 :     return coefficients_ == rhs.coefficients_;
+      71             :   }
+      72           0 :   inline bool operator!=(const Polynomial& rhs) const {
+      73           0 :     return !operator==(rhs);
+      74             :   }
+      75             :   inline Polynomial operator+(const Polynomial& rhs) const {
+      76             :     return Polynomial(coefficients_ + rhs.coefficients_);
+      77             :   }
+      78             :   inline Polynomial& operator+=(const Polynomial& rhs) {
+      79             :     this->coefficients_ += rhs.coefficients_;
+      80             :     return *this;
+      81             :   }
+      82             :   // The product of two polynomials is the convolution of their coefficients.
+      83             :   inline Polynomial operator*(const Polynomial& rhs) const {
+      84             :     return Polynomial(convolve(coefficients_, rhs.coefficients_));
+      85             :   }
+      86             :   // The product of a polynomial with a scalar. Note that polynomials are in
+      87             :   // general not homogeneous, i.e., f(a*t) != a*f(t)
+      88             :   inline Polynomial operator*(const double& rhs) const {
+      89             :     return Polynomial(coefficients_ * rhs);
+      90             :   }
+      91             : 
+      92             :   // Sets up the internal representation from coefficients.
+      93             :   // Coefficients are stored in increasing order with the power of t,
+      94             :   // i.e. c1 + c2*t + c3*t^2 ==> coeffs = [c1 c2 c3]
+      95             :   /* setCoefficients() //{ */
+      96             : 
+      97             :   void setCoefficients(const Eigen::VectorXd& coeffs) {
+      98             :     CHECK_EQ(N_, coeffs.size()) << "Number of coefficients has to match.";
+      99             :     coefficients_ = coeffs;
+     100             :   }
+     101             : 
+     102             :   //}
+     103             : 
+     104             :   // Returns the coefficients for the specified derivative of the
+     105             :   // polynomial as a ROW vector.
+     106             :   /* getCoefficients() //{ */
+     107             : 
+     108       75234 :   Eigen::VectorXd getCoefficients(int derivative = 0) const {
+     109       75234 :     CHECK_LE(derivative, N_);
+     110       75234 :     if (derivative == 0) {
+     111       69294 :       return coefficients_;
+     112             :     } else {
+     113       11880 :       Eigen::VectorXd result(N_);
+     114        5940 :       result.setZero();
+     115        5940 :       result.head(N_ - derivative) =
+     116       11880 :           coefficients_.tail(N_ - derivative).cwiseProduct(base_coefficients_.block(derivative, derivative, 1, N_ - derivative).transpose());
+     117        5940 :       return result;
+     118             :     }
+     119             :   }
+     120             : 
+     121             :   //}
+     122             : 
+     123             :   // Evaluates the polynomial at time t and writes the result.
+     124             :   // Fills in all derivatives up to result.size()-1 (that is, if result is a
+     125             :   // 3-vector, then will fill in derivatives 0, 1, and 2).
+     126             :   /* evaluate() //{ */
+     127             : 
+     128             :   void evaluate(double t, Eigen::VectorXd* result) const {
+     129             :     CHECK_LE(result->size(), N_);
+     130             :     const int max_deg = result->size();
+     131             : 
+     132             :     const int tmp = N_ - 1;
+     133             :     for (int i = 0; i < max_deg; i++) {
+     134             :       Eigen::RowVectorXd row = base_coefficients_.block(i, 0, 1, N_);
+     135             :       double             acc = row[tmp] * coefficients_[tmp];
+     136             :       for (int j = tmp - 1; j >= i; --j) {
+     137             :         acc *= t;
+     138             :         acc += row[j] * coefficients_[j];
+     139             :       }
+     140             :       (*result)[i] = acc;
+     141             :     }
+     142             :   }
+     143             : 
+     144             :   //}
+     145             : 
+     146             :   // Evaluates the specified derivative of the polynomial at time t and returns
+     147             :   // the result (only one value).
+     148             :   /* evaluate() //{ */
+     149             : 
+     150       46492 :   double evaluate(double t, int derivative) const {
+     151       46492 :     if (derivative >= N_) {
+     152           0 :       return 0.0;
+     153             :     }
+     154             :     double             result;
+     155       46492 :     const int          tmp = N_ - 1;
+     156       46492 :     Eigen::RowVectorXd row = base_coefficients_.block(derivative, 0, 1, N_);
+     157       46492 :     result                 = row[tmp] * coefficients_[tmp];
+     158      366842 :     for (int j = tmp - 1; j >= derivative; --j) {
+     159      320350 :       result *= t;
+     160      320350 :       result += row[j] * coefficients_[j];
+     161             :     }
+     162       46492 :     return result;
+     163             :   }
+     164             : 
+     165             :   //}
+     166             : 
+     167             :   // Uses Jenkins-Traub to get all the roots of the polynomial at a certain
+     168             :   // derivative.
+     169             :   bool getRoots(int derivative, Eigen::VectorXcd* roots) const;
+     170             : 
+     171             :   // Finds all candidates for the minimum and maximum between t_start and t_end
+     172             :   // by evaluating the roots of the polynomial's derivative.
+     173             :   static bool selectMinMaxCandidatesFromRoots(double t_start, double t_end, const Eigen::VectorXcd& roots_derivative_of_derivative,
+     174             :                                               std::vector<double>* candidates);
+     175             : 
+     176             :   // Finds all candidates for the minimum and maximum between t_start and t_end
+     177             :   // by computing the roots of the derivative polynomial.
+     178             :   bool computeMinMaxCandidates(double t_start, double t_end, int derivative, std::vector<double>* candidates) const;
+     179             : 
+     180             :   // Evaluates the minimum and maximum of a polynomial between time t_start and
+     181             :   // t_end given the roots of the derivative.
+     182             :   // Returns the minimum and maximum as pair<t, value>.
+     183             :   bool selectMinMaxFromRoots(double t_start, double t_end, int derivative, const Eigen::VectorXcd& roots_derivative_of_derivative,
+     184             :                              std::pair<double, double>* minimum, std::pair<double, double>* maximum) const;
+     185             : 
+     186             :   // Computes the minimum and maximum of a polynomial between time t_start and
+     187             :   // t_end by computing the roots of the derivative polynomial.
+     188             :   // Returns the minimum and maximum as pair<t, value>.
+     189             :   bool computeMinMax(double t_start, double t_end, int derivative, std::pair<double, double>* minimum, std::pair<double, double>* maximum) const;
+     190             : 
+     191             :   // Selects the minimum and maximum of a polynomial among a candidate set.
+     192             :   // Returns the minimum and maximum as pair<t, value>.
+     193             :   bool selectMinMaxFromCandidates(const std::vector<double>& candidates, int derivative, std::pair<double, double>* minimum,
+     194             :                                   std::pair<double, double>* maximum) const;
+     195             : 
+     196             :   // Increase the number of coefficients of this polynomial up to the specified
+     197             :   // degree by appending zeros.
+     198             :   bool getPolynomialWithAppendedCoefficients(int new_N, Polynomial* new_polynomial) const;
+     199             : 
+     200             :   // Computes the base coefficients with the according powers of t, as
+     201             :   // e.g. needed for computation of (in)equality constraints.
+     202             :   // Output: coeffs = vector to write the coefficients to
+     203             :   // Input: polynomial derivative for which the coefficients have to
+     204             :   // be computed
+     205             :   // Input: t = time of evaluation
+     206             :   /* baseCoeffsWithTime() //{ */
+     207             : 
+     208      189010 :   static void baseCoeffsWithTime(int N, int derivative, double t, Eigen::VectorXd* coeffs) {
+     209      189010 :     CHECK_LT(derivative, N);
+     210      189010 :     CHECK_GE(derivative, 0);
+     211             : 
+     212      189010 :     coeffs->resize(N, 1);
+     213      189010 :     coeffs->setZero();
+     214             :     // first coefficient doesn't get multiplied
+     215      189010 :     (*coeffs)[derivative] = base_coefficients_(derivative, derivative);
+     216             : 
+     217      189010 :     if (std::abs(t) < std::numeric_limits<double>::epsilon())
+     218       94505 :       return;
+     219             : 
+     220       94505 :     double t_power = t;
+     221             :     // now multiply increasing power of t towards the right
+     222      756040 :     for (int j = derivative + 1; j < N; j++) {
+     223      661535 :       (*coeffs)[j] = base_coefficients_(derivative, j) * t_power;
+     224      661535 :       t_power      = t_power * t;
+     225             :     }
+     226             :   }
+     227             : 
+     228             :   //}
+     229             : 
+     230             :   // Convenience method to compute the base coefficents with time
+     231             :   // static void baseCoeffsWithTime(const Eigen::MatrixBase<Derived> &
+     232             :   // coeffs, int derivative, double t)
+     233      189010 :   static Eigen::VectorXd baseCoeffsWithTime(int N, int derivative, double t) {
+     234      189010 :     Eigen::VectorXd c(N);
+     235      189010 :     baseCoeffsWithTime(N, derivative, t, &c);
+     236      189010 :     return c;
+     237             :   }
+     238             : 
+     239             :   // Discrete convolution of two vectors.
+     240             :   // convolve(d, k)[m] = sum(d[m - n] * k[n])
+     241             :   static Eigen::VectorXd convolve(const Eigen::VectorXd& data, const Eigen::VectorXd& kernel);
+     242             : 
+     243        2970 :   static inline int getConvolutionLength(int data_size, int kernel_size) {
+     244        2970 :     return data_size + kernel_size - 1;
+     245             :   }
+     246             : 
+     247             :   // Scales the polynomial in time with a scaling factor.
+     248             :   // To stretch out by a factor of 10, pass scaling_factor (b) = 1/10. To shrink
+     249             :   // by a factor of 10, pass scalign factor = 10.
+     250             :   // p_out = a12*b^12*t^12 + a11*b^11*t^11... etc.
+     251             :   void scalePolynomialInTime(double scaling_factor);
+     252             : 
+     253             :   // Offset this polynomial.
+     254             :   void offsetPolynomial(const double offset);
+     255             : 
+     256             : private:
+     257             :   int             N_;
+     258             :   Eigen::VectorXd coefficients_;
+     259             : };
+     260             : 
+     261             : // Static functions to compute base coefficients.
+     262             : 
+     263             : // Computes the base coefficients of the derivatives of the polynomial,
+     264             : // up to order N.
+     265             : Eigen::MatrixXd computeBaseCoefficients(int N);
+     266             : 
+     267             : }  // namespace eth_trajectory_generation
+     268             : 
+     269             : #endif  // ETH_TRAJECTORY_GENERATION_POLYNOMIAL_H_
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.overview.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.overview.html new file mode 100644 index 0000000000..8941f11d19 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.overview.html @@ -0,0 +1,88 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.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_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..20d8f41c578b22045eb35ca05955e271faeccdae GIT binary patch literal 1544 zcmV+j2KV`iP)Bc4 z%_V(_$8LcGcm*SB4@lePLjci0G^E8a1P>?~G@F7HB5&19ASz<@M1ChL#=1j%(vG|Y zIQ-`BlI=9p_%h%X(y=qQhfe{nVq7J?%H!ws9$Q6@P=DOi8!Hib`vLiMarFEK3NXyI zvq@Ya&x#UEz)FU(OM(x;Nbtf&s4<6~tFsl8od$w+>ot}c^0>Zg)(E5(6Mt(|5n*+_ z4W1ho{X`03CmSeAq`@c7c_a%4&MoD+5QL9RWX-i&(tK{x%9M7zDb{-H5&d()>0?dar|aWE z{P}vleqO&XT0ivqzVZ9}RBL-aH9nJl36U;)vJ!P(ah#!s5?p^~cpluAjktE8#Q znRJX(Z3awSRHV~1Tu^dFaSE98^-5xYSaC*372p|R1DHstF`(9vs!!}vQcs0ht7rZp zS72WN4E7SQVgsntmWt&V)$)tR2-2-a>JXa9$29#()Gc9ZPXW~*y+osOjN82_K-CC$ zx!@j?fGu{I1d5`;HxTelaR{zNY36c8@1Ozk0|5?;E~1@zL|YHLcb)NO;sj0c&*K!5 z@YH-)5dL%Ss%M(Uc|mpTa_1D|2fa`wkDO|Dj{9vZZW2=8rz^lG^(nI^S2HDb*HJQRmM z;9`nf%$XO)59{kI&cy|PW76yQ8mN63- zX4Bhdtzgj`m&}DR!w0O-5G!vy+K+NRabDtf^CP8p5$zU_68>t*QkblO#g z#~@`*x#&n&_vB3}cx#noh8r_sfDX=2pD_FO6Xg^pSKYDddP>IpM>7MMYO)F*W zQvD+j^EN$STlGSTB;M4J_T$P9|3YP7hww6zpI8Qe>Jvui1+iL1M&0!~cxCbVda&|M zpg>pSd+DM5S_YNAQop6g+P&ZiDSGt`*Ev|^f!g6bQleuWs+@x4c1+2`Uq2uQR=w9n zxHaxITewm$Sk#cFM(0n_W1Vd#GbPt(Wem4Q_rj7wii4ha7Iqngyx` literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.func-sort-c.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.func-sort-c.html new file mode 100644 index 0000000000..aa78834d32 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.func-sort-c.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial_optimization_linear.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:264065.0 %
    Date:2024-04-09 22:03:33Functions:61154.5 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::PolynomialOptimization<10>::getVertices(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> >*) const0
    eth_trajectory_generation::PolynomialOptimization<10>::getDimension() const0
    eth_trajectory_generation::PolynomialOptimization<10>::getFreeConstraints(std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, std::allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >*) const0
    eth_trajectory_generation::PolynomialOptimization<10>::getDerivativeToOptimize() const0
    eth_trajectory_generation::PolynomialOptimization<10>::getNumberFreeConstraints() const0
    eth_trajectory_generation::PolynomialOptimization<10>::getSegments(std::vector<eth_trajectory_generation::Segment, std::allocator<eth_trajectory_generation::Segment> >*) const11
    eth_trajectory_generation::PolynomialOptimization<10>::getTrajectory(eth_trajectory_generation::Trajectory*) const27
    eth_trajectory_generation::PolynomialOptimization<10>::getSegmentTimes(std::vector<double, std::allocator<double> >*) const259
    eth_trajectory_generation::PolynomialOptimization<10>::getNumberSegments() const633
    eth_trajectory_generation::Constraint::operator<(eth_trajectory_generation::Constraint const&) const12464
    eth_trajectory_generation::Constraint::operator==(eth_trajectory_generation::Constraint const&) const135800
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.func.html new file mode 100644 index 0000000000..bb01ce95ee --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.func.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial_optimization_linear.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:264065.0 %
    Date:2024-04-09 22:03:33Functions:61154.5 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::Constraint::operator==(eth_trajectory_generation::Constraint const&) const135800
    eth_trajectory_generation::Constraint::operator<(eth_trajectory_generation::Constraint const&) const12464
    eth_trajectory_generation::PolynomialOptimization<10>::getSegments(std::vector<eth_trajectory_generation::Segment, std::allocator<eth_trajectory_generation::Segment> >*) const11
    eth_trajectory_generation::PolynomialOptimization<10>::getVertices(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> >*) const0
    eth_trajectory_generation::PolynomialOptimization<10>::getDimension() const0
    eth_trajectory_generation::PolynomialOptimization<10>::getTrajectory(eth_trajectory_generation::Trajectory*) const27
    eth_trajectory_generation::PolynomialOptimization<10>::getSegmentTimes(std::vector<double, std::allocator<double> >*) const259
    eth_trajectory_generation::PolynomialOptimization<10>::getNumberSegments() const633
    eth_trajectory_generation::PolynomialOptimization<10>::getFreeConstraints(std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, std::allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >*) const0
    eth_trajectory_generation::PolynomialOptimization<10>::getDerivativeToOptimize() const0
    eth_trajectory_generation::PolynomialOptimization<10>::getNumberFreeConstraints() const0
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.frameset.html new file mode 100644 index 0000000000..f7c6acfa52 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.html new file mode 100644 index 0000000000..fc90f5c05d --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.html @@ -0,0 +1,401 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial_optimization_linear.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:264065.0 %
    Date:2024-04-09 22:03:33Functions:61154.5 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #ifndef ETH_TRAJECTORY_GENERATION_POLYNOMIAL_OPTIMIZATION_LINEAR_H_
+      22             : #define ETH_TRAJECTORY_GENERATION_POLYNOMIAL_OPTIMIZATION_LINEAR_H_
+      23             : 
+      24             : #include <Eigen/Sparse>
+      25             : #include <tuple>
+      26             : 
+      27             : #include <eth_trajectory_generation/misc.h>
+      28             : #include <eth_trajectory_generation/extremum.h>
+      29             : #include <eth_trajectory_generation/motion_defines.h>
+      30             : #include <eth_trajectory_generation/polynomial.h>
+      31             : #include <eth_trajectory_generation/segment.h>
+      32             : #include <eth_trajectory_generation/trajectory.h>
+      33             : #include <eth_trajectory_generation/vertex.h>
+      34             : 
+      35             : namespace eth_trajectory_generation
+      36             : {
+      37             : 
+      38             : // Implements the unconstrained optimization of paths consisting of
+      39             : // polynomial segments as described in [1]
+      40             : // [1]: Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense
+      41             : // Indoor Environments.
+      42             : //      Charles Richter, Adam Bry, and Nicholas Roy. In ISRR 2013
+      43             : // _N = Number of coefficients of the underlying polynomials.
+      44             : // Polynomial coefficients are stored with increasing powers,
+      45             : // i.e. c_0 + c_1*t ... c_{N-1} * t^{N-1}.
+      46             : template <int _N = 10>
+      47             : class PolynomialOptimization {
+      48             :   static_assert(_N % 2 == 0, "The number of coefficients has to be even.");
+      49             : 
+      50             : public:
+      51             :   enum
+      52             :   {
+      53             :     N = _N
+      54             :   };
+      55             :   static constexpr int                                                      kHighestDerivativeToOptimize = N / 2 - 1;
+      56             :   typedef Eigen::Matrix<double, N, N>                                       SquareMatrix;
+      57             :   typedef std::vector<SquareMatrix, Eigen::aligned_allocator<SquareMatrix>> SquareMatrixVector;
+      58             : 
+      59             :   // Sets up the optimization problem for the specified dimension.
+      60             :   PolynomialOptimization(size_t dimension);
+      61             : 
+      62             :   // Sets up the optimization problem from a vector of Vertex objects and
+      63             :   // a vector of times between the vertices.
+      64             :   // Input: vertices = Vector containing the vertices defining the support
+      65             :   // points and constraints of the path.
+      66             :   // Input: segment_times = Vector containing the time between two vertices.
+      67             :   // Thus, its size is size(vertices) - 1.
+      68             :   // Input: derivative_to_optimize = Specifies the derivative of which the
+      69             :   // cost is optimized.
+      70             :   bool setupFromVertices(const Vertex::Vector& vertices, const std::vector<double>& segment_times, int derivative_to_optimize = kHighestDerivativeToOptimize);
+      71             : 
+      72             :   // Sets up the optimization problem from a vector of positions and a
+      73             :   // vector of times between the via points.
+      74             :   // The optimized derivative is set to the maximum possible based on the given
+      75             :   // N (N/2-1).
+      76             :   // Input: positions = Vector containing the start positions, intermediate
+      77             :   // positions and the final position.
+      78             :   // Input: times = Vector containing the time between two positions. Thus,
+      79             :   // its size is size(positions) - 1.
+      80             :   bool setupFromPositons(const std::vector<double>& positions, const std::vector<double>& times);
+      81             : 
+      82             :   // Wrapper that inverts the mapping matrix (A in [1]) to take advantage
+      83             :   // of its structure.
+      84             :   // Input: A matrix
+      85             :   // Output: Ai inverse of the A matrix
+      86             :   static void invertMappingMatrix(const SquareMatrix& mapping_matrix, SquareMatrix* inverse_mapping_matrix);
+      87             : 
+      88             :   static void setupMappingMatrix(double segment_time, SquareMatrix* A);
+      89             : 
+      90             :   // Computes the cost in the derivative that was specified during
+      91             :   // setupFromVertices().
+      92             :   // The cost is computed as: 0.5*c^T*Q*c
+      93             :   // where c are the coefficients and Q is the cost matrix of each segment.
+      94             :   double computeCost() const;
+      95             : 
+      96             :   // Updates the segment times. The number of times has to be equal to
+      97             :   // the number of vertices that was initially passed during the problem setup.
+      98             :   // This recomputes all cost- and inverse mapping block-matrices and is meant
+      99             :   // to be called during non-linear optimization procedures.
+     100             :   void updateSegmentTimes(const std::vector<double>& segment_times);
+     101             : 
+     102             :   // Solves the linear optimization problem according to [1].
+     103             :   // The solver is re-used for every dimension, which means:
+     104             :   //  - segment times are equal for each dimension.
+     105             :   //  - each dimension has the same type/set of constraints. Their values can of
+     106             :   //    course differ.
+     107             :   bool solveLinear();
+     108             : 
+     109             :   // Returns the trajectory created by the optimization.
+     110             :   // Only valid after solveLinear() is called. This is the preferred external
+     111             :   // interface for getting information back out of the solver.
+     112          27 :   void getTrajectory(Trajectory* trajectory) const {
+     113          27 :     CHECK_NOTNULL(trajectory);
+     114          27 :     trajectory->setSegments(segments_);
+     115          27 :   }
+     116             : 
+     117             :   // Computes the candidates for the maximum magnitude of a single
+     118             :   // segment in the specified derivative.
+     119             :   // In the 1D case, it simply returns the roots of the derivative of the
+     120             :   // segment-polynomial.
+     121             :   // For higher dimensions, e.g. 3D, we need to find the extrema of
+     122             :   // \sqrt{x(t)^2 + y(t)^2 + z(t)^2}
+     123             :   // where x, y, z are polynomials describing the position or the derivative,
+     124             :   // specified by Derivative.
+     125             :   // Taking the derivative yields  2 x \dot{x} + 2 y \dot{y} + 2 z \dot{z},
+     126             :   // which needs to be zero at the extrema. The multiplication of two
+     127             :   // polynomials is a convolution of their coefficients. Re-ordering by their
+     128             :   // powers and addition yields a polynomial, for which we can compute the
+     129             :   // roots. Derivative = Derivative of position, in which to find the maxima.
+     130             :   // Input: segment = Segment to find the maximum.
+     131             :   // Input: t_start = Only maxima >= t_start are returned. Usually set to 0.
+     132             :   // Input: t_stop = Only maxima <= t_stop are returned. Usually set to
+     133             :   // segment time.
+     134             :   // Output: candidates = Vector containing the candidate times for a maximum.
+     135             :   // Returns whether the computation succeeded -- false means no candidates
+     136             :   // were found by Jenkins-Traub.
+     137             :   template <int Derivative>
+     138             :   static bool computeSegmentMaximumMagnitudeCandidates(const Segment& segment, double t_start, double t_stop, std::vector<double>* candidates);
+     139             : 
+     140             :   // Template-free version of above:
+     141             :   static bool computeSegmentMaximumMagnitudeCandidates(int derivative, const Segment& segment, double t_start, double t_stop, std::vector<double>* candidates);
+     142             : 
+     143             :   // Computes the candidates for the maximum magnitude of a single
+     144             :   // segment in the specified derivative.
+     145             :   // Computed by sampling and rather meant for debugging / testing.
+     146             :   // Derivative = Derivative of position, in which to find the maxima.
+     147             :   // Input: segment = Segment to find the maximum.
+     148             :   // Input: t_start = Start time of sampling. Usually set to 0.
+     149             :   // Input: t_stop = End time of sampling. Usually set to segment time.
+     150             :   // Input: sampling_interval = Time between two sampling points.
+     151             :   // Output: candidates = Vector containing the candidate times for a maximum.
+     152             :   template <int Derivative>
+     153             :   static void computeSegmentMaximumMagnitudeCandidatesBySampling(const Segment& segment, double t_start, double t_stop, double sampling_interval,
+     154             :                                                                  std::vector<double>* candidates);
+     155             : 
+     156             :   // Computes the global maximum of the magnitude of the path in the
+     157             :   // specified derivative.
+     158             :   // This uses computeSegmentMaximumMagnitudeCandidates to compute the
+     159             :   // candidates for each segment.
+     160             :   // Derivative = Derivative of position, in which to find the maxima.
+     161             :   // Output: candidates = Vector containing the candidate times for the global
+     162             :   // maximum, i.e. all local maxima.
+     163             :   //                        Optional, can be set to nullptr if not needed.
+     164             :   // Output: return = The global maximum of the path.
+     165             :   template <int Derivative>
+     166             :   Extremum computeMaximumOfMagnitude(std::vector<Extremum>* candidates) const;
+     167             : 
+     168             :   // Template-free version of above.
+     169             :   Extremum computeMaximumOfMagnitude(int derivative, std::vector<Extremum>* candidates) const;
+     170             : 
+     171           0 :   void getVertices(Vertex::Vector* vertices) const {
+     172           0 :     CHECK_NOTNULL(vertices);
+     173           0 :     *vertices = vertices_;
+     174           0 :   }
+     175             : 
+     176             :   // Only for internal use -- always use getTrajectory() instead if you can!
+     177          11 :   void getSegments(Segment::Vector* segments) const {
+     178          11 :     CHECK_NOTNULL(segments);
+     179          11 :     *segments = segments_;
+     180          11 :   }
+     181             : 
+     182         259 :   void getSegmentTimes(std::vector<double>* segment_times) const {
+     183         259 :     CHECK(segment_times != nullptr);
+     184         259 :     *segment_times = segment_times_;
+     185         259 :   }
+     186             : 
+     187           0 :   void getFreeConstraints(std::vector<Eigen::VectorXd>* free_constraints) const {
+     188           0 :     CHECK(free_constraints != nullptr);
+     189           0 :     *free_constraints = free_constraints_compact_;
+     190           0 :   }
+     191             : 
+     192             :   void setFreeConstraints(const std::vector<Eigen::VectorXd>& free_constraints);
+     193             : 
+     194             :   void getFixedConstraints(std::vector<Eigen::VectorXd>* fixed_constraints) const {
+     195             :     CHECK(fixed_constraints != nullptr);
+     196             :     *fixed_constraints = fixed_constraints_compact_;
+     197             :   }
+     198             : 
+     199             :   // Computes the Jacobian of the integral over the squared derivative
+     200             :   // Output: cost_jacobian = Jacobian matrix to write into.
+     201             :   // If C is dynamic, the correct size has to be set.
+     202             :   // Input: t = time of evaluation
+     203             :   // Input: derivative used to compute the cost
+     204             :   static void computeQuadraticCostJacobian(int derivative, double t, SquareMatrix* cost_jacobian);
+     205             : 
+     206           0 :   size_t getDimension() const {
+     207           0 :     return dimension_;
+     208             :   }
+     209         633 :   size_t getNumberSegments() const {
+     210         633 :     return n_segments_;
+     211             :   }
+     212             :   size_t getNumberAllConstraints() const {
+     213             :     return n_all_constraints_;
+     214             :   }
+     215             :   size_t getNumberFixedConstraints() const {
+     216             :     return n_fixed_constraints_;
+     217             :   }
+     218           0 :   size_t getNumberFreeConstraints() const {
+     219           0 :     return n_free_constraints_;
+     220             :   }
+     221           0 :   int getDerivativeToOptimize() const {
+     222           0 :     return derivative_to_optimize_;
+     223             :   }
+     224             : 
+     225             :   // Accessor functions for internal matrices.
+     226             :   void getAInverse(Eigen::MatrixXd* A_inv) const;
+     227             :   void getM(Eigen::MatrixXd* M) const;
+     228             :   void getR(Eigen::MatrixXd* R) const;
+     229             :   // Extras not directly used in the standard optimization:
+     230             :   void getA(Eigen::MatrixXd* A) const;
+     231             :   void getMpinv(Eigen::MatrixXd* M_pinv) const;  // Pseudo-inverse of M.
+     232             : 
+     233             :   void printReorderingMatrix(std::ostream& stream) const;
+     234             : 
+     235             : private:
+     236             :   // Constructs the sparse R (cost) matrix.
+     237             :   void constructR(Eigen::SparseMatrix<double>* R) const;
+     238             : 
+     239             :   // Sets up the matrix (C in [1]) that reorders constraints for the
+     240             :   // optimization problem.
+     241             :   // This matrix is the same for each dimension, i.e. each dimension must have
+     242             :   // the same fixed and free parameters.
+     243             :   void setupConstraintReorderingMatrix();
+     244             : 
+     245             :   // Updates the segments stored internally from the set of compact fixed
+     246             :   // and free constraints.
+     247             :   void updateSegmentsFromCompactConstraints();
+     248             : 
+     249             :   // Matrix consisting of entries with value 1 to reorder free and fixed
+     250             :   // constraints (C in [1]).
+     251             :   Eigen::SparseMatrix<double> constraint_reordering_;
+     252             : 
+     253             :   // Original vertices containing the constraints.
+     254             :   Vertex::Vector vertices_;
+     255             : 
+     256             :   // The actual segments containing the solution.
+     257             :   Segment::Vector segments_;
+     258             : 
+     259             :   // Vector that stores an inverted mapping matrix for each segment
+     260             :   // (A^-1 in [1]).
+     261             :   SquareMatrixVector inverse_mapping_matrices_;
+     262             : 
+     263             :   // Vector that stores the cost matrix for each segment (Q in [1]).
+     264             :   SquareMatrixVector cost_matrices_;
+     265             : 
+     266             :   // Contains the compact form of fixed constraints for each dimension
+     267             :   // (d_f in [1]).
+     268             :   std::vector<Eigen::VectorXd> fixed_constraints_compact_;
+     269             : 
+     270             :   // Contains the compact form of free constraints to optimize for each
+     271             :   // dimension (d_p in [1]).
+     272             :   std::vector<Eigen::VectorXd> free_constraints_compact_;
+     273             : 
+     274             :   std::vector<double> segment_times_;
+     275             : 
+     276             :   // Number of polynomials, e.g 3 for a 3D path.
+     277             :   size_t dimension_;
+     278             : 
+     279             :   int    derivative_to_optimize_;
+     280             :   size_t n_vertices_;
+     281             :   size_t n_segments_;
+     282             : 
+     283             :   size_t n_all_constraints_;
+     284             :   size_t n_fixed_constraints_;
+     285             :   size_t n_free_constraints_;
+     286             : };
+     287             : 
+     288             : // Constraint class that aggregates all constraints from incoming Vertices.
+     289             : struct Constraint
+     290             : {
+     291       12464 :   inline bool operator<(const Constraint& rhs) const {
+     292       12464 :     if (vertex_idx < rhs.vertex_idx)
+     293         330 :       return true;
+     294       12134 :     if (rhs.vertex_idx < vertex_idx)
+     295        7374 :       return false;
+     296             : 
+     297        4760 :     if (constraint_idx < rhs.constraint_idx)
+     298        1125 :       return true;
+     299        3635 :     if (rhs.constraint_idx < constraint_idx)
+     300        2145 :       return false;
+     301        1490 :     return false;
+     302             :   }
+     303             : 
+     304      135800 :   inline bool operator==(const Constraint& rhs) const {
+     305      135800 :     return vertex_idx == rhs.vertex_idx && constraint_idx == rhs.constraint_idx;
+     306             :   }
+     307             : 
+     308             :   size_t                  vertex_idx;
+     309             :   size_t                  constraint_idx;
+     310             :   Vertex::ConstraintValue value;
+     311             : };
+     312             : 
+     313             : }  // namespace eth_trajectory_generation
+     314             : 
+     315             : #include "eth_trajectory_generation/impl/polynomial_optimization_linear_impl.h"
+     316             : 
+     317             : #endif  // ETH_TRAJECTORY_GENERATION_POLYNOMIAL_OPTIMIZATION_LINEAR_H_
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.overview.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.overview.html new file mode 100644 index 0000000000..e67ea62f98 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.overview.html @@ -0,0 +1,100 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.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 + + +
    + Top

    + Overview +
    + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_linear.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..d2de30a36cde7c341f1c8788fbb702e209bb071f GIT binary patch literal 1749 zcmV;`1}gc9P)}A5{ zowGyIsWoVoa78rxVel*n&)7E<-8Z00SR-u^mb5Lxf@OnnTE|(A-=x&>{0y_7=k126 z$Kv@A2w!EY2zFsglgztA+=%Wdn4Xj;vd#JH-;(NcZqpXu^TA&N2bszOaJfB>gl<%*&_xl2(eIWr(=tpr$4KOeul<e!CV zp{z5;1g5omgPWF7ros&XLgHA@B$oIwi%Ws0qO*9T&#@h6QjR9jAB|h;)+1wd^yT=( zV0EOP2j(VyNV7$kmKi_Ngceb19xXg6iONkRiD5^ltvN=ON=%4#76&!2$g4Z2n`59K&BJCNZ~=C!maNhzlz*1}#|vO+=k zxYP^50wG*6OkSO?X?}#X4dV{7um`jHS^pd7h%qS)reM+ZB-IhRqm{xOM?8&=5zdjq zh$(q#HiU$k#q1kaCqfTG5_yW2FqnQTqcc{A3B1%q%}O6Y95goQZh{>E3JQThiXl`b zCKJtsp(_WkQnA~sR;`9Tz51soTHG+|sY}s6MhZznS?Q1z!9Hc;jL9en7$eCmO9EL zLLd#{jG`SKPsUN7|3N+^j;7c?w*07JR_p`v8PT+bDVK=ok+su=^Eyrz&C?~WC+z6? z!(VZNBi1y^uo)Yd&?wHL^0b>%-*c1g6#gt)rM*MC&h4e3ml}0Q!clmRI1YNRg{`JS zbY1)@W4bO{F=mQxr6oO=6!QAI*jF@d^9|;gUq0gxblmJ@?oQc)u=%pdKwb|jeh&bo rZg>{*KFn&InEx00000NkvXXu0mjfu|8cK literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.func-sort-c.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.func-sort-c.html new file mode 100644 index 0000000000..d83bc52fd9 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.func-sort-c.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial_optimization_nonlinear.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:77100.0 %
    Date:2024-04-09 22:03:33Functions:33100.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::getPolynomialOptimizationRef()11
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::getTrajectory(eth_trajectory_generation::Trajectory*) const11
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::getOptimizationInfo() const33
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.func.html new file mode 100644 index 0000000000..4e6d332219 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.func.html @@ -0,0 +1,92 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial_optimization_nonlinear.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:77100.0 %
    Date:2024-04-09 22:03:33Functions:33100.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::getPolynomialOptimizationRef()11
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::getTrajectory(eth_trajectory_generation::Trajectory*) const11
    eth_trajectory_generation::PolynomialOptimizationNonLinear<10>::getOptimizationInfo() const33
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.frameset.html new file mode 100644 index 0000000000..12ac5e7f12 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.html new file mode 100644 index 0000000000..72ec64d9d5 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.html @@ -0,0 +1,399 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - polynomial_optimization_nonlinear.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:77100.0 %
    Date:2024-04-09 22:03:33Functions:33100.0 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #ifndef ETH_TRAJECTORY_GENERATION_POLYNOMIAL_OPTIMIZATION_NONLINEAR_H_
+      22             : #define ETH_TRAJECTORY_GENERATION_POLYNOMIAL_OPTIMIZATION_NONLINEAR_H_
+      23             : 
+      24             : #include <memory>
+      25             : #include <nlopt.hpp>
+      26             : 
+      27             : #include <eth_trajectory_generation/polynomial_optimization_linear.h>
+      28             : 
+      29             : namespace eth_trajectory_generation
+      30             : {
+      31             : 
+      32             : constexpr double kOptimizationTimeLowerBound = 0.01;
+      33             : 
+      34             : // Class holding all important parameters for nonlinear optimization.
+      35             : struct NonlinearOptimizationParameters
+      36             : {
+      37             :   // Default parameters should be reasonable enough to use without further
+      38             :   // fine-tuning.
+      39             : 
+      40             :   // Stopping criteria, if objective function changes less than absolute value.
+      41             :   // Disabled if negative.
+      42             :   double f_abs = -1;
+      43             : 
+      44             :   // Stopping criteria, if objective function changes less than relative value.
+      45             :   // Disabled if negative.
+      46             :   double f_rel = 0.05;
+      47             : 
+      48             :   // Stopping criteria, if state changes less than relative value.
+      49             :   // Disabled if negative.
+      50             :   double x_rel = -1;
+      51             : 
+      52             :   // Stopping criteria, if state changes less than absolute value.
+      53             :   // Disabled if negative.
+      54             :   double x_abs = -1;
+      55             : 
+      56             :   // Determines a fraction of the initial guess as initial step size.
+      57             :   // Heuristic value if negative.
+      58             :   double initial_stepsize_rel = 0.1;
+      59             : 
+      60             :   // Absolute tolerance, within an equality constraint is considered as met.
+      61             :   double equality_constraint_tolerance = 1.0e-3;
+      62             : 
+      63             :   // Absolute tolerance, within an inequality constraint is considered as met.
+      64             :   double inequality_constraint_tolerance = 0.1;
+      65             : 
+      66             :   // Maximum number of iterations. Disabled if negative.
+      67             :   int max_iterations = 3000;
+      68             : 
+      69             :   // max execution time in secs
+      70             :   double max_time = 1.0;
+      71             : 
+      72             :   // Penalty for the segment time.
+      73             :   double time_penalty = 500.0;
+      74             : 
+      75             :   // Optimization algorithm used by nlopt, see
+      76             :   // http://ab-initio.mit.edu/wiki/index.php/NLopt_Algorithms
+      77             :   // Previous value was nlopt::LN_SBPLX, but we found that BOBYQA has slightly
+      78             :   // better convergence and lower run-time in the unit tests.
+      79             :   nlopt::algorithm algorithm = nlopt::LN_BOBYQA;
+      80             : 
+      81             :   // Random seed, if an optimization algorithm involving random numbers
+      82             :   // is used (e.g. nlopt::GN_ISRES).
+      83             :   // If set to a value < 0, a "random" (getTimeofday) value for the seed
+      84             :   // is chosen.
+      85             :   int random_seed = 0;
+      86             : 
+      87             :   // Decide whether to use soft constraints.
+      88             :   bool use_soft_constraints = true;
+      89             : 
+      90             :   // Weights the relative violation of a soft constraint.
+      91             :   double soft_constraint_weight = 100.0;
+      92             : 
+      93             :   enum TimeAllocMethod
+      94             :   {
+      95             :     kSquaredTime               = 0,
+      96             :     kRichterTime               = 1,
+      97             :     kMellingerOuterLoop        = 2,
+      98             :     kSquaredTimeAndConstraints = 3,
+      99             :     kRichterTimeAndConstraints = 4,
+     100             :     kUnknown                   = 5,
+     101             :   } time_alloc_method = kSquaredTimeAndConstraints;
+     102             : 
+     103             :   bool print_debug_info                 = false;
+     104             :   bool print_debug_info_time_allocation = false;
+     105             : };
+     106             : 
+     107             : struct OptimizationInfo
+     108             : {
+     109             :   int                     n_iterations          = 0;
+     110             :   int                     stopping_reason       = nlopt::FAILURE;
+     111             :   double                  cost_trajectory       = 0.0;
+     112             :   double                  cost_time             = 0.0;
+     113             :   double                  cost_soft_constraints = 0.0;
+     114             :   double                  optimization_time     = 0.0;
+     115             :   std::map<int, Extremum> maxima;
+     116             : };
+     117             : 
+     118             : std::ostream& operator<<(std::ostream& stream, const OptimizationInfo& val);
+     119             : 
+     120             : // Implements a nonlinear optimization of the unconstrained optimization
+     121             : // of paths consisting of polynomial segments as described in [1]
+     122             : // [1]: Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense
+     123             : // Indoor Environments.
+     124             : // Charles Richter, Adam Bry, and Nicholas Roy. In ISRR 2013
+     125             : // _N specifies the number of coefficients for the underlying polynomials.
+     126             : template <int _N = 10>
+     127             : class PolynomialOptimizationNonLinear {
+     128             :   static_assert(_N % 2 == 0, "The number of coefficients has to be even.");
+     129             : 
+     130             : public:
+     131             :   enum
+     132             :   {
+     133             :     N = _N
+     134             :   };
+     135             : 
+     136             :   // Sets up the nonlinear optimization problem.
+     137             :   // Input: dimension = Spatial dimension of the problem. Usually 1 or 3.
+     138             :   // Input: parameters = Parameters for the optimization problem.
+     139             :   // Input: optimize_time_only = Specifies whether the optimization is run
+     140             :   // over the segment times only.
+     141             :   // If true, only the segment times are optimization parameters, and the
+     142             :   // remaining free parameters are found by solving the linear optimization
+     143             :   // problem with the given segment times in every iteration.
+     144             :   // If false, both segment times and free derivatives become optimization
+     145             :   // variables. The latter case is theoretically correct, but may result in
+     146             :   // more iterations.
+     147             :   PolynomialOptimizationNonLinear(size_t dimension, const NonlinearOptimizationParameters& parameters);
+     148             : 
+     149             :   // Sets up the optimization problem from a vector of Vertex objects and
+     150             :   // a vector of times between the vertices.
+     151             :   // Input: vertices = Vector containing the vertices defining the support
+     152             :   // points and constraints of the path.
+     153             :   // Input: segment_times = Vector containing an initial guess of the time
+     154             :   // between two vertices. Thus, its size is size(vertices) - 1.
+     155             :   // Input: derivative_to_optimize = Specifies the derivative of which the
+     156             :   // cost is optimized.
+     157             :   bool setupFromVertices(const Vertex::Vector& vertices, const std::vector<double>& segment_times,
+     158             :                          int derivative_to_optimize = PolynomialOptimization<N>::kHighestDerivativeToOptimize);
+     159             : 
+     160             :   // Adds a constraint for the maximum of magnitude to the optimization
+     161             :   // problem.
+     162             :   // Input: derivative_order = Order of the derivative, for which the
+     163             :   // constraint should be checked. Usually velocity (=1) or acceleration (=2).
+     164             :   // maximum_value = Maximum magnitude of the specified derivative.
+     165             :   bool addMaximumMagnitudeConstraint(int dimension, int derivative_order, double maximum_value);
+     166             : 
+     167             :   // Solves the linear optimization problem according to [1].
+     168             :   // The solver is re-used for every dimension, which means:
+     169             :   //  - segment times are equal for each dimension.
+     170             :   //  - each dimension has the same type/set of constraints. Their values can of
+     171             :   // course differ.
+     172             :   bool solveLinear();
+     173             : 
+     174             :   // Runs the optimization until one of the stopping criteria in
+     175             :   // NonlinearOptimizationParameters and the constraints are met.
+     176             :   int optimize();
+     177             : 
+     178             :   // Get the resulting trajectory out -- prefer this as the main method
+     179             :   // to get the results of the optimization, over getting the reference
+     180             :   // to the linear optimizer.
+     181          11 :   void getTrajectory(Trajectory* trajectory) const {
+     182          11 :     poly_opt_.getTrajectory(trajectory);
+     183          11 :   }
+     184             : 
+     185             :   // Returns a const reference to the underlying linear optimization
+     186             :   // object.
+     187             :   const PolynomialOptimization<N>& getPolynomialOptimizationRef() const {
+     188             :     return poly_opt_;
+     189             :   }
+     190             : 
+     191             :   // Returns a non-const reference to the underlying linear optimization
+     192             :   // object.
+     193          11 :   PolynomialOptimization<N>& getPolynomialOptimizationRef() {
+     194          11 :     return poly_opt_;
+     195             :   }
+     196             : 
+     197          33 :   OptimizationInfo getOptimizationInfo() const {
+     198          33 :     return optimization_info_;
+     199             :   }
+     200             : 
+     201             :   // Functions for optimization, but may be useful for diagnostics outside.
+     202             :   // Gets the trajectory cost (same as the cost in the linear problem).
+     203             :   double getCost() const;
+     204             : 
+     205             :   // Gets the cost including the soft constraints and time costs, should be
+     206             :   // the same cost function as used in the full optimization. Returns the same
+     207             :   // metrics regardless of time estimation method set.
+     208             :   double getTotalCostWithSoftConstraints() const;
+     209             : 
+     210             :   void scaleSegmentTimesWithViolation();
+     211             : 
+     212             : private:
+     213             :   // Holds the data for constraint evaluation, since these methods are
+     214             :   // static.
+     215             :   struct ConstraintData
+     216             :   {
+     217             :     PolynomialOptimizationNonLinear<N>* this_object;
+     218             :     int                                 derivative;
+     219             :     int                                 dimension;
+     220             :     double                              value;
+     221             :   };
+     222             : 
+     223             :   // Objective function for the time-only version.
+     224             :   // Input: segment_times = Segment times in the current iteration.
+     225             :   // Input: gradient = Gradient of the objective function w.r.t. changes of
+     226             :   // parameters. We can't compute the gradient analytically here.
+     227             :   // Thus, only gradient-free optimization methods are possible.
+     228             :   // Input: Custom data pointer = In our case, it's an ConstraintData object.
+     229             :   // Output: Cost = based on the parameters passed in.
+     230             :   static double objectiveFunctionTime(const std::vector<double>& segment_times, std::vector<double>& gradient, void* data);
+     231             : 
+     232             :   // Objective function for the time-only Mellinger Outer Loop.
+     233             :   // Input: segment_times = Segment times in the current iteration.
+     234             :   // Input: gradient = Gradient of the objective function w.r.t. changes of
+     235             :   // parameters. We can't compute the gradient analytically here.
+     236             :   // Thus, only gradient-free optimization methods are possible.
+     237             :   // Input: Custom data pointer = In our case, it's an ConstraintData object.
+     238             :   // Output: Cost = based on the parameters passed in.
+     239             :   static double objectiveFunctionTimeMellingerOuterLoop(const std::vector<double>& segment_times, std::vector<double>& gradient, void* data);
+     240             : 
+     241             :   // Objective function for the version optimizing segment times and free
+     242             :   // derivatives.
+     243             :   // Input: optimization_variables = Optimization variables times in the
+     244             :   // current iteration.
+     245             :   // The variables (time, derivatives) are stacked as follows: [segment_times
+     246             :   // derivatives_dim_0 ... derivatives_dim_N]
+     247             :   // Input: gradient = Gradient of the objective function wrt. changes of
+     248             :   // parameters. We can't compute the gradient analytically here.
+     249             :   // Thus, only gradient free optimization methods are possible.
+     250             :   // Input: data = Custom data pointer. In our case, it's an ConstraintData
+     251             :   // object.
+     252             :   // Output: Cost based on the parameters passed in.
+     253             :   static double objectiveFunctionTimeAndConstraints(const std::vector<double>& optimization_variables, std::vector<double>& gradient, void* data);
+     254             : 
+     255             :   // Evaluates the maximum magnitude constraint at the current value of
+     256             :   // the optimization variables.
+     257             :   // All input parameters are ignored, all information is contained in data.
+     258             :   static double evaluateMaximumMagnitudeConstraint(const std::vector<double>& optimization_variables, std::vector<double>& gradient, void* data);
+     259             : 
+     260             :   // Does the actual optimization work for the time-only version.
+     261             :   int optimizeTime();
+     262             :   int optimizeTimeMellingerOuterLoop();
+     263             : 
+     264             :   // Does the actual optimization work for the full optimization version.
+     265             :   int optimizeTimeAndFreeConstraints();
+     266             : 
+     267             :   // Evaluates the maximum magnitude constraints as soft constraints and
+     268             :   // returns a cost, depending on the violation of the constraints.
+     269             :   // cost_i = min(maximum_cost, exp(abs_violation_i / max_allowed_i * weight))
+     270             :   //  cost = sum(cost_i)
+     271             :   // Input: inequality_constraints = Vector of ConstraintData shared_ptrs,
+     272             :   // describing the constraints.
+     273             :   // Input: weight = Multiplicative weight of the constraint violation.
+     274             :   // Input: maximum_cost = Upper bound of the cost. Necessary, since exp of a
+     275             :   // high violation can end up in inf.
+     276             :   // Output: Sum of the costs per constraint.
+     277             :   double evaluateMaximumMagnitudeAsSoftConstraint(const std::vector<std::shared_ptr<ConstraintData>>& inequality_constraints, double weight,
+     278             :                                                   double maximum_cost = 1.0e12) const;
+     279             : 
+     280             :   // Set lower and upper bounds on the optimization parameters
+     281             :   void setFreeEndpointDerivativeHardConstraints(const Vertex::Vector& vertices, std::vector<double>* lower_bounds, std::vector<double>* upper_bounds);
+     282             : 
+     283             :   // Computes the gradients by doing forward difference!
+     284             :   double getCostAndGradientMellinger(std::vector<double>* gradients);
+     285             : 
+     286             :   // Computes the total trajectory time.
+     287             :   static double computeTotalTrajectoryTime(const std::vector<double>& segment_times);
+     288             : 
+     289             :   // nlopt optimization object.
+     290             :   std::shared_ptr<nlopt::opt> nlopt_;
+     291             : 
+     292             :   // Underlying linear optimization object.
+     293             :   PolynomialOptimization<N> poly_opt_;
+     294             : 
+     295             :   // Parameters for the nonlinear optimzation.
+     296             :   NonlinearOptimizationParameters optimization_parameters_;
+     297             : 
+     298             :   // Holds the data for evaluating inequality constraints.
+     299             :   std::vector<std::shared_ptr<ConstraintData>> inequality_constraints_;
+     300             : 
+     301             :   OptimizationInfo optimization_info_;
+     302             : };
+     303             : 
+     304             : }  // namespace eth_trajectory_generation
+     305             : 
+     306             : namespace nlopt
+     307             : {
+     308             : // Convenience function that turns nlopt's return values into something
+     309             : // readable.
+     310             : std::string returnValueToString(int return_value);
+     311             : }  // namespace nlopt
+     312             : 
+     313             : #endif  // ETH_TRAJECTORY_GENERATION_POLYNOMIAL_OPTIMIZATION_NONLINEAR_H_
+     314             : 
+     315             : #include "eth_trajectory_generation/impl/polynomial_optimization_nonlinear_impl.h"
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.overview.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.overview.html new file mode 100644 index 0000000000..544371f979 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.overview.html @@ -0,0 +1,99 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.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 + + +
    + Top

    + Overview +
    + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/polynomial_optimization_nonlinear.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..af2627eca3e13ea8303f37289d8c1aed6ac71a83 GIT binary patch literal 1665 zcmV-{27dX8P)W04pPc!`wd_wupKW@Fh9j=@2I@*_PKmq_|u^854p zr>_ew*T-*4UBW`#p{oe`fj|nP;g~|N*)pwyq5y(F?j{9922ZlT#f!-!q%XQ- zZy_9ha&;k`#%q&sNA%p{zX1v=IEj2PVRL5Pk9y<+MzN7OqHRz;m8>pC#@ zIj&osD7z?WEy<2wC){_`ndo9A3@-&&kh}^)ISt8thsbS#`$-+|%dV(n_L{n?HQ^t? z@$c$*Y_7Du7F`ZyH4uuW%Qz)MzP=dWqhm-?T>76zG%D0P3iL#GWp}F7uk3dUhj)Y6 zx%)AKJ>jatqIW$7cYt?w7k|D zqW#YAQ8r4&5o|2DQyUQ5R%>ZNi8UhL_hC^QJSIZ3=5iD&^>!3UAXu`ce;dCF?+uipsvUE2GbxiPobpT5$4YDgm9ykVsY>PS`!Ehi*rH; zeSuI19FuP|V=zDBLX(ViNU^AIwh-Jt+Wvvvj?1cTdx_^Q%opk?pd5RvCG^?`Hl;oD zBpxx+sUL4yYZI)s)|RJ2^df8x7@RfY?Rka{bOrt*MKRXE&wG~FI{mW8*X6J7)E@|O z!4%K^T$iv!6plOXM$PY*`tNZjIHaIfNn4IYOeY)%d)E<@)@+nQuZT1yo3bVljoCa? z<=%=8Z4`v!U8c=?Qm2u%zaUaSP^=Q0at6an!O>C)R-b2UwOuQqh zlU8jL$61u@jy-r{vYXI)fX{GQTp{yYf$71i@A={qt zM@d|?1&+yn41~2`{1F->xme5gbga+9#eysx1o*=u4}Mf?4XHpOG12t-^YisX>&yQC zdWQ9QK7X3^?5xN0yZ3gFaB*4LlPa<5-N;)BDSjmDtkFcd$fBU&!xpFQ;THYs`EV@^{`TD3w<=}O0L z8;Z+Q7$8d&BvIclIgF zlD3y3XO0h)GKAW%I2VRwGB&9iFOZYTCQ!Ro{APV13zdv0H`Z43MiIlpmEB6p5#Y@z z`ycRT`P**+?ls8D5)m#qv(`|2WH2w0=#|EYd8`bkNG?(JSHv$g{%Zc z8SF_s#|1>6qU|`I2G1;dIBQpAvS>~_?A;bWJ~mSrOw2MY7;N`J87)tY@r``ziC(fD z7@CHWsTt1Blc81b-Og*IBSoBkPISlD_SiOgz$F_h!bs7aq?A+$M-t@~7xtqqe4vb+ z>sZ&GP;C>Ra-rdx$79Bum`G^Uv=Low7k}7ZN~xA2r6yeQ-I5XD0yg6lfy5ynea;sX zBN0za2Pjd;vP&chKCUI6s>q|ts%XGMXR58GV|XT*E{cYC2Za41C6xl3q8;M-cW0uV zZx4OEwM?0|2;U_-euMKwbgGcQ1^~LCkn^jy+mHhXPJ9Ms#ItH8Vb)zHTx$8*oC1hBoE;*nMzR~&z3&x0DjZ8^p00000 LNkvXXu0mjf^^+Nm literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func-sort-c.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func-sort-c.html new file mode 100644 index 0000000000..eb51fd83a5 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func-sort-c.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - segment.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:1313100.0 %
    Date:2024-04-09 22:03:33Functions:66100.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::Segment::Segment(int, int)16
    eth_trajectory_generation::Segment::N() const290
    eth_trajectory_generation::Segment::Segment(eth_trajectory_generation::Segment const&)526
    eth_trajectory_generation::Segment::D() const1115
    eth_trajectory_generation::Segment::getTime() const14553
    eth_trajectory_generation::Segment::setTime(double)75109
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func.html new file mode 100644 index 0000000000..0101e83862 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.func.html @@ -0,0 +1,104 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - segment.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:1313100.0 %
    Date:2024-04-09 22:03:33Functions:66100.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::Segment::setTime(double)75109
    eth_trajectory_generation::Segment::Segment(eth_trajectory_generation::Segment const&)526
    eth_trajectory_generation::Segment::Segment(int, int)16
    eth_trajectory_generation::Segment::D() const1115
    eth_trajectory_generation::Segment::N() const290
    eth_trajectory_generation::Segment::getTime() const14553
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.frameset.html new file mode 100644 index 0000000000..a87cffca2c --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.html new file mode 100644 index 0000000000..46e86a442a --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.html @@ -0,0 +1,231 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - segment.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:1313100.0 %
    Date:2024-04-09 22:03:33Functions:66100.0 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #ifndef ETH_TRAJECTORY_GENERATION_SEGMENT_H_
+      22             : #define ETH_TRAJECTORY_GENERATION_SEGMENT_H_
+      23             : 
+      24             : #include <Eigen/Core>
+      25             : #include <chrono>
+      26             : #include <map>
+      27             : #include <vector>
+      28             : 
+      29             : #include <eth_trajectory_generation/misc.h>
+      30             : #include <eth_trajectory_generation/extremum.h>
+      31             : #include <eth_trajectory_generation/motion_defines.h>
+      32             : #include <eth_trajectory_generation/polynomial.h>
+      33             : 
+      34             : namespace eth_trajectory_generation
+      35             : {
+      36             : 
+      37             : constexpr double kNumNSecPerSec = 1.0e9;
+      38             : constexpr double kNumSecPerNsec = 1.0e-9;
+      39             : 
+      40             : // Class holding the properties of parametric segment of a path.
+      41             : // Time of the segment and one polynomial for each dimension.
+      42             : //    X------------X---------------X
+      43             : //  vertex             segment
+      44             : class Segment {
+      45             : public:
+      46             :   typedef std::vector<Segment> Vector;
+      47             : 
+      48          16 :   Segment(int N, int D) : time_(0.0), N_(N), D_(D) {
+      49          16 :     polynomials_.resize(D_, Polynomial(N_));
+      50          16 :   }
+      51         526 :   Segment(const Segment& segment) = default;
+      52             : 
+      53             :   bool        operator==(const Segment& rhs) const;
+      54             :   inline bool operator!=(const Segment& rhs) const {
+      55             :     return !operator==(rhs);
+      56             :   }
+      57             : 
+      58        1115 :   int D() const {
+      59        1115 :     return D_;
+      60             :   }
+      61         290 :   int N() const {
+      62         290 :     return N_;
+      63             :   }
+      64       14553 :   double getTime() const {
+      65       14553 :     return time_;
+      66             :   }
+      67             :   uint64_t getTimeNSec() const {
+      68             :     return static_cast<uint64_t>(kNumNSecPerSec * time_);
+      69             :   }
+      70             : 
+      71       75109 :   void setTime(double time_sec) {
+      72       75109 :     time_ = time_sec;
+      73       75109 :   }
+      74             :   void setTimeNSec(uint64_t time_ns) {
+      75             :     time_ = time_ns * kNumSecPerNsec;
+      76             :   }
+      77             : 
+      78             :   Polynomial& operator[](size_t idx);
+      79             : 
+      80             :   const Polynomial& operator[](size_t idx) const;
+      81             : 
+      82             :   const Polynomial::Vector& getPolynomialsRef() const {
+      83             :     return polynomials_;
+      84             :   }
+      85             : 
+      86             :   Eigen::VectorXd evaluate(double t, int derivative_order = derivative_order::POSITION) const;
+      87             : 
+      88             :   // Computes the candidates for the minimum and maximum magnitude of a single
+      89             :   // segment in the specified derivative. In the 1D case, it simply returns the
+      90             :   // roots of the derivative of the segment-polynomial. For higher dimensions,
+      91             :   // e.g. 3D, we need to find the extrema of \sqrt{x(t)^2 + y(t)^2 + z(t)^2}
+      92             :   // where x, y, z are polynomials describing the position or the derivative,
+      93             :   // specified by Derivative. Taking the derivative yields
+      94             :   // 2 x \dot{x} + 2 y \dot{y} + 2 z \dot{z}, which needs to be zero at the
+      95             :   // extrema. The multiplication of two polynomials is a convolution of their
+      96             :   // coefficients. Re-ordering by their powers and addition yields a polynomial,
+      97             :   // for which we can compute the roots.
+      98             :   // Input: derivative = Derivative of position, in which to find the maxima.
+      99             :   // Input: t_start = Only maxima >= t_start are returned. Usually set to 0.
+     100             :   // Input: t_stop = Only maxima <= t_stop are returned. Usually set to segment
+     101             :   // time.
+     102             :   // Input: dimensions = Vector containing the dimensions that are evaluated.
+     103             :   // Usually [0, 1, 2] for position, [3] for yaw.
+     104             :   // Output: candidates = Vector containing the candidate extrema times.
+     105             :   // Returns whether the computation succeeded -- false means no candidates
+     106             :   // were found by Jenkins-Traub.
+     107             :   bool computeMinMaxMagnitudeCandidateTimes(int derivative, double t_start, double t_end, const std::vector<int>& dimensions,
+     108             :                                             std::vector<double>* candidate_times) const;
+     109             : 
+     110             :   // Convenience function. Additionally evaluates the candidate times.
+     111             :   bool computeMinMaxMagnitudeCandidates(int derivative, double t_start, double t_end, const std::vector<int>& dimensions,
+     112             :                                         std::vector<Extremum>* candidates) const;
+     113             : 
+     114             :   // Convenience function. Evaluates the magnitudes between t_start and t_end
+     115             :   // for a set of candidates for given dimensions.
+     116             :   bool selectMinMaxMagnitudeFromCandidates(int derivative, double t_start, double t_end, const std::vector<int>& dimensions,
+     117             :                                            const std::vector<Extremum>& candidates, Extremum* minimum, Extremum* maximum) const;
+     118             : 
+     119             :   // Split a segment to get a segment with the specified dimension.
+     120             :   bool getSegmentWithSingleDimension(int dimension, Segment* new_segment) const;
+     121             :   // Compose this segment and another segment to a new segment.
+     122             :   bool getSegmentWithAppendedDimension(const Segment& segment_to_append, Segment* new_segment) const;
+     123             : 
+     124             :   // Offset this segment by vector A_r_B.
+     125             :   bool offsetSegment(const Eigen::VectorXd& A_r_B);
+     126             : 
+     127             : protected:
+     128             :   Polynomial::Vector polynomials_;
+     129             :   double             time_;
+     130             : 
+     131             : private:
+     132             :   int N_;  // Number of coefficients.
+     133             :   int D_;  // Number of dimensions.
+     134             : };
+     135             : 
+     136             : // Prints the properties of the segment.
+     137             : // Polynomial coefficients are printed with increasing powers,
+     138             : // i.e. c_0 + c_1*t ... c_{N-1} * t^{N-1}
+     139             : void printSegment(std::ostream& stream, const Segment& s, int derivative);
+     140             : 
+     141             : std::ostream& operator<<(std::ostream& stream, const Segment& s);
+     142             : 
+     143             : std::ostream& operator<<(std::ostream& stream, const std::vector<Segment>& segments);
+     144             : 
+     145             : }  // namespace eth_trajectory_generation
+     146             : 
+     147             : #endif  // ETH_TRAJECTORY_GENERATION_SEGMENT_H_
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.overview.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.overview.html new file mode 100644 index 0000000000..29e01f7518 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.overview.html @@ -0,0 +1,57 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.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_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/segment.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..c0cf8ac373362295e39f36e7e97f854ffabb560e GIT binary patch literal 898 zcmV-|1AY97P)qCQt0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpsx>f-$Lbk2Nx_8Z!NP<}kez$_|%JH$sJ;T2N#6 zA4TRBCxARYg`UcV6xiRcNLJVQg-wI*;GNKHMtLqVNG-OuLM_e45`T$DGf!4fP4kHf zv6WaSS>4yW)w^!8W%nTac{Br%8v(R!JMtDlC1T*xHX93X0gyKW()oZn1G0{?M6hPh zY*qy*Hd@o+wjm8ChpggGI3Rz4JFyErUK>fjfnK#XCFu35Gp7~Y=ZL@Ez1gBID?f|o zog>Tv(Kni4JyHphmk4!@1x#=xA)L-0v(b`>T%&9BKdCMst7vX0Po1l;#Am#8p{9}ps&%DGM=eajI(>5;26mSvx Y1D!habWi%)_y7O^07*qoM6N<$f*BB;1ONa4 literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func-sort-c.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func-sort-c.html new file mode 100644 index 0000000000..a08b5f9d75 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func-sort-c.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - timing.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:0390.0 %
    Date:2024-04-09 22:03:33Functions:0100.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::Add(double)0
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::Accumulator()0
    eth_trajectory_generation::timing::TimerMapValue::TimerMapValue()0
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::RollingMean() const0
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::LazyVariance() const0
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::TotalSamples() const0
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::Max() const0
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::Min() const0
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::Sum() const0
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::Mean() const0
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func.html new file mode 100644 index 0000000000..52c67e0b6c --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.func.html @@ -0,0 +1,120 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - timing.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:0390.0 %
    Date:2024-04-09 22:03:33Functions:0100.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::Add(double)0
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::Accumulator()0
    eth_trajectory_generation::timing::TimerMapValue::TimerMapValue()0
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::RollingMean() const0
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::LazyVariance() const0
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::TotalSamples() const0
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::Max() const0
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::Min() const0
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::Sum() const0
    eth_trajectory_generation::timing::Accumulator<double, double, 50>::Mean() const0
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.frameset.html new file mode 100644 index 0000000000..2699d2a11a --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.html new file mode 100644 index 0000000000..9ad8645552 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.html @@ -0,0 +1,322 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - timing.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:0390.0 %
    Date:2024-04-09 22:03:33Functions:0100.0 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : /* Adapted from Paul Furgale Schweizer Messer sm_timing */
+      22             : 
+      23             : #ifndef ETH_TRAJECTORY_GENERATION_TIMING_H_
+      24             : #define ETH_TRAJECTORY_GENERATION_TIMING_H_
+      25             : 
+      26             : #include <algorithm>
+      27             : #include <chrono>
+      28             : #include <limits>
+      29             : #include <map>
+      30             : #include <string>
+      31             : #include <vector>
+      32             : 
+      33             : namespace eth_trajectory_generation
+      34             : {
+      35             : namespace timing
+      36             : {
+      37             : 
+      38             : template <typename T, typename Total, int N>
+      39             : class Accumulator {
+      40             : public:
+      41           0 :   Accumulator() : window_samples_(0), total_samples_(0), window_sum_(0), sum_(0), min_(std::numeric_limits<T>::max()), max_(std::numeric_limits<T>::min()) {
+      42           0 :   }
+      43             : 
+      44           0 :   void Add(T sample) {
+      45           0 :     if (window_samples_ < N) {
+      46           0 :       samples_[window_samples_++] = sample;
+      47           0 :       window_sum_ += sample;
+      48             :     } else {
+      49           0 :       T& oldest = samples_[window_samples_++ % N];
+      50           0 :       window_sum_ += sample - oldest;
+      51           0 :       oldest = sample;
+      52             :     }
+      53           0 :     sum_ += sample;
+      54           0 :     ++total_samples_;
+      55           0 :     if (sample > max_) {
+      56           0 :       max_ = sample;
+      57             :     }
+      58           0 :     if (sample < min_) {
+      59           0 :       min_ = sample;
+      60             :     }
+      61           0 :   }
+      62             : 
+      63           0 :   int TotalSamples() const {
+      64           0 :     return total_samples_;
+      65             :   }
+      66             : 
+      67           0 :   double Sum() const {
+      68           0 :     return sum_;
+      69             :   }
+      70             : 
+      71           0 :   double Mean() const {
+      72           0 :     return sum_ / total_samples_;
+      73             :   }
+      74             : 
+      75           0 :   double RollingMean() const {
+      76           0 :     return window_sum_ / std::min(window_samples_, N);
+      77             :   }
+      78             : 
+      79           0 :   double Max() const {
+      80           0 :     return max_;
+      81             :   }
+      82             : 
+      83           0 :   double Min() const {
+      84           0 :     return min_;
+      85             :   }
+      86             : 
+      87           0 :   double LazyVariance() const {
+      88           0 :     if (window_samples_ == 0) {
+      89           0 :       return 0.0;
+      90             :     }
+      91           0 :     double var  = 0;
+      92           0 :     double mean = RollingMean();
+      93           0 :     for (int i = 0; i < std::min(window_samples_, N); ++i) {
+      94           0 :       var += (samples_[i] - mean) * (samples_[i] - mean);
+      95             :     }
+      96           0 :     var /= std::min(window_samples_, N);
+      97           0 :     return var;
+      98             :   }
+      99             : 
+     100             : private:
+     101             :   int   window_samples_;
+     102             :   int   total_samples_;
+     103             :   Total window_sum_;
+     104             :   Total sum_;
+     105             :   T     min_;
+     106             :   T     max_;
+     107             :   T     samples_[N];
+     108             : };
+     109             : 
+     110             : struct TimerMapValue
+     111             : {
+     112           0 :   TimerMapValue() {
+     113           0 :   }
+     114             : 
+     115             :   // Create an accumulator with specified window size.
+     116             :   Accumulator<double, double, 50> acc_;
+     117             : };
+     118             : 
+     119             : // A class that has the timer interface but does nothing. Swapping this in in
+     120             : // place of the Timer class (say with a typedef) should allow one to disable
+     121             : // timing. Because all of the functions are inline, they should just disappear.
+     122             : class DummyTimer {
+     123             : public:
+     124             :   DummyTimer(size_t /*handle*/, bool /*constructStopped*/ = false) {
+     125             :   }
+     126             :   DummyTimer(std::string const& /*tag*/, bool /*constructStopped*/ = false) {
+     127             :   }
+     128             :   ~DummyTimer() {
+     129             :   }
+     130             : 
+     131             :   void Start() {
+     132             :   }
+     133             :   void Stop() {
+     134             :   }
+     135             :   bool IsTiming() {
+     136             :     return false;
+     137             :   }
+     138             : };
+     139             : 
+     140             : class Timer {
+     141             : public:
+     142             :   Timer(size_t handle, bool constructStopped = false);
+     143             :   Timer(std::string const& tag, bool constructStopped = false);
+     144             :   ~Timer();
+     145             : 
+     146             :   void Start();
+     147             :   void Stop();
+     148             :   bool IsTiming() const;
+     149             : 
+     150             : private:
+     151             :   std::chrono::time_point<std::chrono::system_clock> time_;
+     152             : 
+     153             :   bool   timing_;
+     154             :   size_t handle_;
+     155             : };
+     156             : 
+     157             : class Timing {
+     158             : public:
+     159             :   typedef std::map<std::string, size_t> map_t;
+     160             :   friend class Timer;
+     161             :   // Definition of static functions to query the timers.
+     162             :   static size_t       GetHandle(std::string const& tag);
+     163             :   static std::string  GetTag(size_t handle);
+     164             :   static double       GetTotalSeconds(size_t handle);
+     165             :   static double       GetTotalSeconds(std::string const& tag);
+     166             :   static double       GetMeanSeconds(size_t handle);
+     167             :   static double       GetMeanSeconds(std::string const& tag);
+     168             :   static size_t       GetNumSamples(size_t handle);
+     169             :   static size_t       GetNumSamples(std::string const& tag);
+     170             :   static double       GetVarianceSeconds(size_t handle);
+     171             :   static double       GetVarianceSeconds(std::string const& tag);
+     172             :   static double       GetMinSeconds(size_t handle);
+     173             :   static double       GetMinSeconds(std::string const& tag);
+     174             :   static double       GetMaxSeconds(size_t handle);
+     175             :   static double       GetMaxSeconds(std::string const& tag);
+     176             :   static double       GetHz(size_t handle);
+     177             :   static double       GetHz(std::string const& tag);
+     178             :   static void         Print(std::ostream& out);
+     179             :   static std::string  Print();
+     180             :   static std::string  SecondsToTimeString(double seconds);
+     181             :   static void         Reset();
+     182             :   static const map_t& GetTimers() {
+     183             :     return Instance().tag_map_;
+     184             :   }
+     185             : 
+     186             : private:
+     187             :   void AddTime(size_t handle, double seconds);
+     188             : 
+     189             :   static Timing& Instance();
+     190             : 
+     191             :   Timing();
+     192             :   ~Timing();
+     193             : 
+     194             :   typedef std::vector<TimerMapValue> list_t;
+     195             : 
+     196             :   list_t timers_;
+     197             :   map_t  tag_map_;
+     198             :   size_t max_tag_length_;
+     199             : };
+     200             : 
+     201             : #if DISABLE_TIMING
+     202             : typedef DummyTimer DebugTimer;
+     203             : #else
+     204             : typedef Timer DebugTimer;
+     205             : #endif
+     206             : 
+     207             : // Small timer for benchmarking.
+     208             : class MiniTimer {
+     209             : public:
+     210             :   MiniTimer() : start_(std::chrono::system_clock::now()) {
+     211             :   }
+     212             : 
+     213             :   void start() {
+     214             :     start_ = std::chrono::system_clock::now();
+     215             :   }
+     216             : 
+     217             :   double stop() {
+     218             :     end_ = std::chrono::system_clock::now();
+     219             :     return getTime();
+     220             :   }
+     221             : 
+     222             :   double getTime() const {
+     223             :     if (end_ < start_) {
+     224             :       return 0.0;
+     225             :     }
+     226             :     std::chrono::duration<double> duration = end_ - start_;
+     227             :     return duration.count();
+     228             :   }
+     229             : 
+     230             : private:
+     231             :   std::chrono::time_point<std::chrono::system_clock> start_;
+     232             :   std::chrono::time_point<std::chrono::system_clock> end_;
+     233             : };
+     234             : 
+     235             : }  // namespace timing
+     236             : }  // namespace eth_trajectory_generation
+     237             : 
+     238             : #endif  // ETH_TRAJECTORY_GENERATION_TIMING_H_
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.overview.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.overview.html new file mode 100644 index 0000000000..aacb270f7b --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.overview.html @@ -0,0 +1,80 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.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 + + +
    + Top

    + Overview +
    + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/timing.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..a87d0279cff82695695e5e828e2cb5660ff6702e GIT binary patch literal 1027 zcmV+e1pNDnP)qV$0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp3|4SAVEPQ+U?k>Gr~oH8O~(On$!a8K!f(>M&KP+hONYis*a8W!PeeCW^pcJp zp#ogtn7oJV?f3#Pdw?3zIWUaJWEtL)f*q2dN|-RaNZdp6%u%G0A-ZVAUIq?7`IJ~c z%n4qX#Ah5OL5&Fdfh(eKscZmEX{$iLj{8KriS8aBv383TjwWz>-j-N{R2AwOo3EFa z4sKt6y&{G15-P9G>iPHzUoJX)unQPDU(8R7V^-y@=NNp6e$^DMQuNNi5E)NJ2TQ%e z@ofAUJ$K%jx1vW17T`Hokae^bOu#q4vBVb09yEa`D>-#iGRC%L&6qzE{Xi*5-ptKO zk&$B9qNAHwvgPlhqbd3^9m7>w9vySJj?mBPXa>8E^(i6eul zS$V}WkxDR^FaEK$c4HC616wU>X(nZ=7iZ;wmo1ANT|7A? z+eANcHAA0wRx=#hGz91`m~|;YT?(-fum$c-HTR~8NKt$Y^eu;6PKfvyizUicP}>RgSZ8Y>m3 zNgwIhi63x;?zpG{)pXK1Dh?V09S&Eu{2d+lavYXq&rv_@B_JNPX#K=TO+IQeRbXQ{ z^}sCJ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - trajectory.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:254161.0 %
    Date:2024-04-09 22:03:33Functions:71258.3 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::Trajectory::clear()0
    eth_trajectory_generation::Trajectory::getSegments(std::vector<eth_trajectory_generation::Segment, std::allocator<eth_trajectory_generation::Segment> >*) const0
    eth_trajectory_generation::Trajectory::K() const0
    eth_trajectory_generation::Trajectory::N() const0
    eth_trajectory_generation::Trajectory::segments() const0
    eth_trajectory_generation::Trajectory::getMaxTime() const22
    eth_trajectory_generation::Trajectory::getMinTime() const22
    eth_trajectory_generation::Trajectory::addSegments(std::vector<eth_trajectory_generation::Segment, std::allocator<eth_trajectory_generation::Segment> > const&)27
    eth_trajectory_generation::Trajectory::setSegments(std::vector<eth_trajectory_generation::Segment, std::allocator<eth_trajectory_generation::Segment> > const&)27
    eth_trajectory_generation::Trajectory::Trajectory()27
    eth_trajectory_generation::Trajectory::~Trajectory()27
    eth_trajectory_generation::Trajectory::D() const1419
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.func.html new file mode 100644 index 0000000000..21ee70be1a --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.func.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - trajectory.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:254161.0 %
    Date:2024-04-09 22:03:33Functions:71258.3 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::Trajectory::addSegments(std::vector<eth_trajectory_generation::Segment, std::allocator<eth_trajectory_generation::Segment> > const&)27
    eth_trajectory_generation::Trajectory::setSegments(std::vector<eth_trajectory_generation::Segment, std::allocator<eth_trajectory_generation::Segment> > const&)27
    eth_trajectory_generation::Trajectory::clear()0
    eth_trajectory_generation::Trajectory::Trajectory()27
    eth_trajectory_generation::Trajectory::~Trajectory()27
    eth_trajectory_generation::Trajectory::getMaxTime() const22
    eth_trajectory_generation::Trajectory::getMinTime() const22
    eth_trajectory_generation::Trajectory::getSegments(std::vector<eth_trajectory_generation::Segment, std::allocator<eth_trajectory_generation::Segment> >*) const0
    eth_trajectory_generation::Trajectory::D() const1419
    eth_trajectory_generation::Trajectory::K() const0
    eth_trajectory_generation::Trajectory::N() const0
    eth_trajectory_generation::Trajectory::segments() const0
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.frameset.html new file mode 100644 index 0000000000..c189d69eb8 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.html new file mode 100644 index 0000000000..5b06892404 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.html @@ -0,0 +1,261 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - trajectory.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:254161.0 %
    Date:2024-04-09 22:03:33Functions:71258.3 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #ifndef ETH_TRAJECTORY_GENERATION_TRAJECTORY_H_
+      22             : #define ETH_TRAJECTORY_GENERATION_TRAJECTORY_H_
+      23             : 
+      24             : #include <eth_trajectory_generation/extremum.h>
+      25             : #include <eth_trajectory_generation/segment.h>
+      26             : #include <eth_trajectory_generation/vertex.h>
+      27             : 
+      28             : namespace eth_trajectory_generation
+      29             : {
+      30             : 
+      31             : // Holder class for trajectories of D dimensions, of K segments, and
+      32             : // polynomial order N-1. (N=12 -> 11th order polynomial, with 12 coefficients).
+      33             : class Trajectory {
+      34             : public:
+      35          27 :   Trajectory() : D_(0), N_(0), max_time_(0.0) {
+      36          27 :   }
+      37          27 :   ~Trajectory() {
+      38          27 :   }
+      39             : 
+      40             :   bool        operator==(const Trajectory& rhs) const;
+      41             :   inline bool operator!=(const Trajectory& rhs) const {
+      42             :     return !operator==(rhs);
+      43             :   }
+      44             : 
+      45        1419 :   int D() const {
+      46        1419 :     return D_;
+      47             :   }
+      48           0 :   int N() const {
+      49           0 :     return N_;
+      50             :   }
+      51           0 :   int K() const {
+      52           0 :     return segments_.size();
+      53             :   }
+      54             : 
+      55             :   bool empty() const {
+      56             :     return segments_.empty();
+      57             :   }
+      58           0 :   void clear() {
+      59           0 :     segments_.clear();
+      60           0 :     D_        = 0;
+      61           0 :     N_        = 0;
+      62           0 :     max_time_ = 0.0;
+      63           0 :   }
+      64             : 
+      65          27 :   void setSegments(const Segment::Vector& segments) {
+      66          27 :     CHECK(!segments.empty());
+      67             :     // Reset states.
+      68          27 :     D_        = segments.front().D();
+      69          27 :     N_        = segments.front().N();
+      70          27 :     max_time_ = 0.0;
+      71          27 :     segments_.clear();
+      72             : 
+      73          27 :     addSegments(segments);
+      74          27 :   }
+      75             : 
+      76          27 :   void addSegments(const Segment::Vector& segments) {
+      77         290 :     for (const Segment& segment : segments) {
+      78         263 :       CHECK_EQ(segment.D(), D_);
+      79         263 :       CHECK_EQ(segment.N(), N_);
+      80         263 :       max_time_ += segment.getTime();
+      81             :     }
+      82          27 :     segments_.insert(segments_.end(), segments.begin(), segments.end());
+      83          27 :   }
+      84             : 
+      85           0 :   void getSegments(Segment::Vector* segments) const {
+      86           0 :     CHECK_NOTNULL(segments);
+      87           0 :     *segments = segments_;
+      88           0 :   }
+      89             : 
+      90           0 :   const Segment::Vector& segments() const {
+      91           0 :     return segments_;
+      92             :   }
+      93             : 
+      94          22 :   double getMinTime() const {
+      95          22 :     return 0.0;
+      96             :   }
+      97          22 :   double getMaxTime() const {
+      98          22 :     return max_time_;
+      99             :   }
+     100             :   std::vector<double> getSegmentTimes() const;
+     101             : 
+     102             :   // Functions to create new trajectories by splitting (getting a NEW trajectory
+     103             :   // with a single dimension) or compositing (create a new trajectory with
+     104             :   // another trajectory appended).
+     105             :   Trajectory getTrajectoryWithSingleDimension(int dimension) const;
+     106             :   bool       getTrajectoryWithAppendedDimension(const Trajectory& trajectory_to_append, Trajectory* new_trajectory) const;
+     107             : 
+     108             :   // Add trajectories with same dimensions and coefficients to this trajectory.
+     109             :   bool addTrajectories(const std::vector<Trajectory>& trajectories, Trajectory* merged) const;
+     110             : 
+     111             :   // Offset this trajectory by vector A_r_B.
+     112             :   bool offsetTrajectory(const Eigen::VectorXd& A_r_B);
+     113             : 
+     114             :   // Evaluate the vertex constraint at time t.
+     115             :   Vertex getVertexAtTime(double t, int max_derivative_order) const;
+     116             :   // Evaluate the vertex constraint at start time.
+     117             :   Vertex getStartVertex(int max_derivative_order) const;
+     118             :   // Evaluate the vertex constraint at goal time.
+     119             :   Vertex getGoalVertex(int max_derivative_order) const;
+     120             :   // Evaluate all underlying vertices.
+     121             :   bool getVertices(int max_derivative_order_pos, int max_derivative_order_yaw, Vertex::Vector* pos_vertices, Vertex::Vector* yaw_vertices) const;
+     122             :   bool getVertices(int max_derivative_order, Vertex::Vector* vertices) const;
+     123             : 
+     124             :   // Evaluation functions.
+     125             :   // Evaluate at a single time, and a single derivative. Return type of
+     126             :   // dimension D.
+     127             :   Eigen::VectorXd evaluate(double t, int derivative_order = derivative_order::POSITION) const;
+     128             : 
+     129             :   // Evaluates the trajectory in a specified range and derivative.
+     130             :   // Outputs are a vector of the sampled values (size of VectorXd is D) by
+     131             :   // time and optionally the actual sampling times.
+     132             :   void evaluateRange(double t_start, double t_end, double dt, int derivative_order, std::vector<Eigen::VectorXd>* result,
+     133             :                      std::vector<double>* sampling_times = nullptr) const;
+     134             : 
+     135             :   // Compute the analytic minimum and maximum of magnitude for a given
+     136             :   // derivative and dimensions, e.g., [0, 1, 2] for position or [3] for yaw.
+     137             :   // Returns false in case of extremum calculation failure.
+     138             :   bool computeMinMaxMagnitude(int derivative, const std::vector<int>& dimensions, Extremum* minimum, Extremum* maximum, int seg) const;
+     139             : 
+     140             :   // Compute the analytic minimum and maximum of magnitude for a given
+     141             :   // derivative and dimensions, e.g., [0, 1, 2] for position or [3] for yaw.
+     142             :   // Returns false in case of extremum calculation failure.
+     143             :   bool computeMinMaxMagnitude(int derivative, const std::vector<int>& dimensions, Extremum* minimum, Extremum* maximum) const;
+     144             : 
+     145             :   // Compute max velocity and max acceleration. Shorthand for the method above.
+     146             :   bool computeMaxDerivativesHorizontal(double* v_max, double* a_max, double* j_max, int seg) const;
+     147             :   bool computeMaxDerivativesHorizontal(double* v_max, double* a_max, double* j_max) const;
+     148             : 
+     149             :   bool computeMaxDerivativesVertical(double* v_max, double* a_max, double* j_max, int seg) const;
+     150             :   bool computeMaxDerivativesVertical(double* v_max, double* a_max, double* j_max) const;
+     151             : 
+     152             :   bool computeMaxDerivativesHeading(double* v_max, double* a_max, double* j_max, int seg) const;
+     153             :   bool computeMaxDerivativesHeading(double* v_max, double* a_max, double* j_max) const;
+     154             : 
+     155             :   // This method SCALES the segment times evenly.
+     156             :   bool scaleSegmentTimes(double scaling);
+     157             : 
+     158             :   // This method SCALES the segment times evenly to ensure that the trajectory
+     159             :   // is feasible given the provided v_max and a_max. Does not change the shape
+     160             :   // of the trajectory, and only *increases* segment times.
+     161             :   bool scaleSegmentTimesToMeetConstraints(const double v_max_translation_horizontal, const double v_max_translation_vertical,
+     162             :                                           const double a_max_translation_horizontal, const double a_max_translation_vertical,
+     163             :                                           const double j_max_translation_horizontal, const double j_max_translation_vertical, const double v_max_heading,
+     164             :                                           const double a_max_heading, const double j_max_heading);
+     165             : 
+     166             : private:
+     167             :   int    D_;         // Number of dimensions.
+     168             :   int    N_;         // Number of coefficients.
+     169             :   double max_time_;  // Time at the end of the trajectory.
+     170             : 
+     171             :   // K is number of segments...
+     172             :   Segment::Vector segments_;
+     173             : };
+     174             : 
+     175             : }  // namespace eth_trajectory_generation
+     176             : 
+     177             : #endif  // ETH_TRAJECTORY_GENERATION_TRAJECTORY_H_
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.overview.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.overview.html new file mode 100644 index 0000000000..141f034ccd --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.overview.html @@ -0,0 +1,65 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.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_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/trajectory.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..447774c6994623d59cbcbe230f12d8f6ecc7d63f GIT binary patch literal 973 zcmV;;12X)HP)v&3Wzmrx*$hH ze!P74J0ZsskNcCj1s+!kKd;yQx?ZnyEZccr*Y$HvU=cV|B|xWXl24dk7g|b~&sR*d%z$fB(lZ2pyoY;I$TQYKJel1xnpRx*<8cFz>}fZJYy4F8C@~jb z67b`MpQ?>M@}iHb2kVwH>A2DW#@*)+8o){JUpzT<<`Ol1jD7fn>0_759#8K)u}?GZ zTbp@I4|}7}w5-bVvddVPxr8=iX)g@CwB`jJ^1Ay0BJY@GIsHuGAWQG85weqg)18e9 zKgSHbKddb=s!taUDppy;>hR@kp>R;UfQ1K6!co)M%V1%zSY|t{HKx_M2SsX{E^R}_ z0**As_fR8h)iu`k(cTV&a>??p(MZT1J))*2!Rg> zox@CyxYU7SZl#b11@j`!3;A>{1UY3>8q-lmJ~V63#;Sr_y~2 zlXt8R;JS~N#ztY!v;}<2w9>?i9Y9ZJ)aaBnTg|?id*3`O>X@d5P+s + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - vertex.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:6875.0 %
    Date:2024-04-09 22:03:33Functions:3475.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::Vertex::D() const0
    eth_trajectory_generation::Vertex::cBegin() const181
    eth_trajectory_generation::Vertex::Vertex(unsigned long)390
    eth_trajectory_generation::Vertex::cEnd() const438
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.func.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.func.html new file mode 100644 index 0000000000..e6f4194934 --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.func.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - vertex.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:6875.0 %
    Date:2024-04-09 22:03:33Functions:3475.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::Vertex::Vertex(unsigned long)390
    eth_trajectory_generation::Vertex::D() const0
    eth_trajectory_generation::Vertex::cEnd() const438
    eth_trajectory_generation::Vertex::cBegin() const181
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.frameset.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.frameset.html new file mode 100644 index 0000000000..a88ee3993a --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.html new file mode 100644 index 0000000000..2bc26b900d --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.html @@ -0,0 +1,256 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/include/eth_trajectory_generation - vertex.h (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:6875.0 %
    Date:2024-04-09 22:03:33Functions:3475.0 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #ifndef ETH_TRAJECTORY_GENERATION_VERTEX_H_
+      22             : #define ETH_TRAJECTORY_GENERATION_VERTEX_H_
+      23             : 
+      24             : #include <Eigen/Core>
+      25             : #include <chrono>
+      26             : #include <map>
+      27             : #include <vector>
+      28             : #include <eth_trajectory_generation/misc.h>
+      29             : #include <eth_trajectory_generation/motion_defines.h>
+      30             : #include <eth_trajectory_generation/polynomial.h>
+      31             : 
+      32             : namespace eth_trajectory_generation
+      33             : {
+      34             : 
+      35             : // A vertex describes the properties of a support point of a path.
+      36             : // A vertex has a set of constraints, the derivative of position a value, that
+      37             : // have to be matched during optimization procedures. In case of a
+      38             : // multi-dimensional vertex (mostly 3D), the constraint for a derivative
+      39             : // exists in all dimensions, but can have different values in each dimension.
+      40             : //     X------------X---------------X
+      41             : //   vertex             segment
+      42             : class Vertex {
+      43             : public:
+      44             :   typedef std::vector<Vertex>             Vector;
+      45             :   typedef Eigen::VectorXd                 ConstraintValue;
+      46             :   typedef std::pair<int, ConstraintValue> Constraint;
+      47             :   typedef std::map<int, ConstraintValue>  Constraints;
+      48             : 
+      49             :   // Constructs an empty vertex and sets time_to_next and
+      50             :   // derivative_to_optimize to zero.
+      51         390 :   Vertex(size_t dimension) : D_(dimension) {
+      52         390 :   }
+      53             : 
+      54           0 :   int D() const {
+      55           0 :     return D_;
+      56             :   }
+      57             : 
+      58             :   // Adds a constraint for the specified derivative order with the given
+      59             :   // value. If this is a multi-dimensional vertex, all dimensions are
+      60             :   // set to the same value.
+      61             :   inline void addConstraint(int derivative_order, double value) {
+      62             :     constraints_[derivative_order] = ConstraintValue::Constant(D_, value);
+      63             :   }
+      64             : 
+      65             :   // Adds a constraint for the derivative specified in type with the given
+      66             :   // values in the constraint vector. The dimension has to match the derivative.
+      67             :   void addConstraint(int type, const Eigen::VectorXd& constraint);
+      68             : 
+      69             :   // Removes a constraint for the derivative specified in type. Returns false if
+      70             :   // constraint was not set.
+      71             :   bool removeConstraint(int type);
+      72             : 
+      73             :   // Sets a constraint for position and sets all derivatives up to
+      74             :   // (including) up_to_derivative to zero. Convenience method for
+      75             :   // beginning / end vertices. up_to_derivative should be set to
+      76             :   // getHighestDerivativeFromN(N), where N is the order of your polynomial.
+      77             :   void makeStartOrEnd(const Eigen::VectorXd& constraint, int up_to_derivative);
+      78             : 
+      79             :   void makeStartOrEnd(double value, int up_to_derivative) {
+      80             :     makeStartOrEnd(Eigen::VectorXd::Constant(D_, value), up_to_derivative);
+      81             :   }
+      82             : 
+      83             :   // Returns whether the vertex has a constraint for the specified derivative
+      84             :   // order.
+      85             :   bool hasConstraint(int derivative_order) const;
+      86             : 
+      87             :   // Passes the value of the constraint for derivative order to *value,
+      88             :   // and returns whether the constraint is set.
+      89             :   bool getConstraint(int derivative_order, Eigen::VectorXd* constraint) const;
+      90             : 
+      91             :   // Returns a const iterator to the first constraint.
+      92         181 :   typename Constraints::const_iterator cBegin() const {
+      93         181 :     return constraints_.begin();
+      94             :   }
+      95             : 
+      96             :   // Returns a const iterator to the end of the constraints,
+      97             :   // i.e. one after the last element.
+      98         438 :   typename Constraints::const_iterator cEnd() const {
+      99         438 :     return constraints_.end();
+     100             :   }
+     101             : 
+     102             :   // Returns the number of constraints.
+     103             :   size_t getNumberOfConstraints() const {
+     104             :     return constraints_.size();
+     105             :   }
+     106             : 
+     107             :   // Checks if both lhs and rhs are equal up to tol in case of double values.
+     108             :   bool isEqualTol(const Vertex& rhs, double tol) const;
+     109             : 
+     110             :   // Get subdimension vertex.
+     111             :   bool getSubdimension(const std::vector<size_t>& subdimensions, int max_derivative_order, Vertex* subvertex) const;
+     112             : 
+     113             : private:
+     114             :   int         D_;
+     115             :   Constraints constraints_;
+     116             : };
+     117             : 
+     118             : std::ostream& operator<<(std::ostream& stream, const Vertex& v);
+     119             : 
+     120             : std::ostream& operator<<(std::ostream& stream, const std::vector<Vertex>& vertices);
+     121             : 
+     122             : // Makes a rough estimate based on v_max and a_max about the time
+     123             : // required to get from one vertex to the next. Uses the current preferred
+     124             : // method.
+     125             : std::vector<double> estimateSegmentTimes(const Vertex::Vector& vertices, const double v_max_horizontal, const double v_max_vertical,
+     126             :                                          const double a_max_horizontal, const double a_max_vertical, const double j_max_horizontal, const double j_max_vertical,
+     127             :                                          const double heading_speed_max, const double heading_acc_max);
+     128             : 
+     129             : // Calculate the velocity assuming instantaneous constant acceleration a_max
+     130             : // and straight line rest-to-rest trajectories.
+     131             : // The time_factor \in [1..Inf] increases the allocated time making the segments
+     132             : // slower and thus feasibility more likely. This method does not take into
+     133             : // account the start and goal velocity and acceleration.
+     134             : std::vector<double> estimateSegmentTimesVelocityRamp(const Vertex::Vector& vertices, double v_max, double a_max, double time_factor = 1.0);
+     135             : 
+     136             : std::vector<double> estimateSegmentTimesEuclidean(const Vertex::Vector& vertices, const double v_max_horizontal, const double v_max_vertical,
+     137             :                                                   const double a_max_horizontal, const double a_max_vertical, const double j_max_horizontal,
+     138             :                                                   const double j_max_vertical, const double heading_speed_max, const double heading_acc_max);
+     139             : 
+     140             : std::vector<double> estimateSegmentTimesBaca(const Vertex::Vector& vertices, const double v_max_horizontal, const double v_max_vertical,
+     141             :                                              const double a_max_horizontal, const double a_max_vertical, const double j_max_horizontal,
+     142             :                                              const double j_max_vertical, const double heading_speed_max, const double heading_acc_max);
+     143             : 
+     144             : double computeTimeVelocityRamp(const Eigen::VectorXd& start, const Eigen::VectorXd& goal, double v_max, double a_max);
+     145             : 
+     146             : inline int getHighestDerivativeFromN(int N) {
+     147             :   return N / 2 - 1;
+     148             : }
+     149             : 
+     150             : // Creates random vertices for position within minimum_position and
+     151             : // maximum_position.
+     152             : // Vertices at the beginning and end have only fixed constraints with their
+     153             : // derivative set to zero, while  all vertices in between have position as fixed
+     154             : // constraint and the derivatives are left free.
+     155             : // Input: maximum_derivative = The maximum derivative to be set to zero for
+     156             : // beginning and end.
+     157             : // Input: n_segments = Number of segments of the resulting trajectory. Number
+     158             : // of vertices is n_segments + 1.
+     159             : // Input: minimum_position = Minimum position of the space to sample.
+     160             : // Input: maximum_position = Maximum position of the space to sample.
+     161             : // Input: seed = Initial seed for random number generation.
+     162             : // Output: return = Vector containing n_segments + 1 vertices.
+     163             : Vertex::Vector createRandomVertices(int maximum_derivative, size_t n_segments, const Eigen::VectorXd& minimum_position, const Eigen::VectorXd& maximum_position,
+     164             :                                     size_t seed = 0);
+     165             : 
+     166             : Vertex::Vector createSquareVertices(int maximum_derivative, const Eigen::Vector3d& center, double side_length, int rounds);
+     167             : 
+     168             : // Conveninence function to create 1D vertices.
+     169             : Vertex::Vector createRandomVertices1D(int maximum_derivative, size_t n_segments, double minimum_position, double maximum_position, size_t seed = 0);
+     170             : }  // namespace eth_trajectory_generation
+     171             : 
+     172             : #endif  // ETH_TRAJECTORY_GENERATION_VERTEX_H_
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.overview.html b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.overview.html new file mode 100644 index 0000000000..81b47fa20f --- /dev/null +++ b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.overview.html @@ -0,0 +1,63 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.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_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.png b/mrs_uav_trajectory_generation/include/eth_trajectory_generation/vertex.h.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..9440efee0a24bd84a6b04358990b63f53bf2afb6 GIT binary patch literal 1052 zcmV+%1mpXOP);EA6l25621U z{QS>31QIyFRlu^Bb}%y0&j^46TqI)$xTG~Q5uuZE)+oa-5clM41P{zOc38HM^^%Ou zLI7N$SgZ%K_W2<}ERY>*aSY*6G(!~>WDq&1n;<4)^JG4f7qdo4%sSAQfZd%>mxR-W zz2|`|tjP`+WEgnKjezs4XDLpZrv*mn9`JKh=GrwqQhy#iTbAqsiI6}Q3g=!01UP5R z7tZ%W=Kz+B%TlDI-jyw-$m&YoaYa4(97}*O#g@t%G2r62+h&JXj_DXIA2`|@dX9KZ z*m2tb&etOu3=rm;+9d$8`o6)mK#^bz=q)T$MU(49sF>S~!63zNKj(U!@ArE-yWg6% z-5%}5Xl-+_9r%2|_SgIMdXCrje7@iBfA0&}0lXr`z$Jr2+Mww~(DYG*Q^MIiGa057 z6FY7g!Z~x^xd@^x2OQF_dL&33aNj6rjd{Q!=j`KZicC@EU?#gm7i+#$r`QLXQ{2E_ z;39gyK?)(VCKpO!9Z)pcg^y$kWHfe^%%%!#MnoyfCbi{~Edr#Wjad#JxS6x=tp~vp zppf`Oq_q(w>DEe&xxStxCQdR{ISZHGRWRePM7T+0bgBUXr`Y=lSLJ^vqj*e$n4Jmr zicHt0E~SWbM@HlO)wq`|Oxe!=4VAMYQ6HjpFAeJ9I?&!{Jh0!8W;@rflt~i26%cX< zIhdbgtsA8qt^3M+O>3^?gJjPxu}}SHkb?~PPF{oD1IZq652wbk*;J63d8T`hS?icS zB2w2!mw>iuPII0{Q^~(p(MmMbWsdK{^2QJwB|iK$McrMXm~5Revko0$fmBJLU0SDr z#IDn6Zc3S2AJK%HSA4I<_ZKFEW2P0bPqAwr9eGqbIp?PniqXso15Sq~B_gAynK86y zGQ7&}#6zG~J-H8-ac9QhjoBB>)29e~98u7R0t>(u7xrYV0$<>4`oa(T^83WkajYnV@119#14xwdl%lbf>2cdFZfIL+q z&XKChn4)O9wz77$wXY1AT1KrKy&q4xl6xB7_oS0io_X`E>&NgVAVK73i1uld_*y*7 z(n{kXppSn=@+Z*f|aQu05) WL=9arSkejr0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generationHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:452103643.6 %
    Date:2024-04-09 22:03:33Functions:3110529.5 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
    motion_defines.cpp +
    0.0%
    +     		0.0 %0 / 280.0 %0 / 4
    timing.cpp +
    0.0%
    +     		0.0 %0 / 1110.0 %0 / 30
    trajectory_sampling.cpp +
    42.0%42.0%
    +     		42.0 %29 / 6928.6 %2 / 7
    <unnamed>42.0 %29 / 6928.6 %2 / 7
    vertex.cpp +
    50.0%50.0%
    +     		50.0 %135 / 27035.3 %6 / 17
    <unnamed>50.0 %135 / 27035.3 %6 / 17
    segment.cpp +
    40.0%40.0%
    +     		40.0 %54 / 13546.2 %6 / 13
    <unnamed>40.0 %54 / 13546.2 %6 / 13
    trajectory.cpp +
    57.3%57.3%
    +     		57.3 %180 / 31447.8 %11 / 23
    <unnamed>57.3 %180 / 31447.8 %11 / 23
    polynomial.cpp +
    49.5%49.5%
    +     		49.5 %54 / 10954.5 %6 / 11
    <unnamed>49.5 %54 / 10954.5 %6 / 11
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail-sort-l.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail-sort-l.html new file mode 100644 index 0000000000..1424d68070 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail-sort-l.html @@ -0,0 +1,202 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generationHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:452103643.6 %
    Date:2024-04-09 22:03:33Functions:3110529.5 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
    motion_defines.cpp +
    0.0%
    +     		0.0 %0 / 280.0 %0 / 4
    timing.cpp +
    0.0%
    +     		0.0 %0 / 1110.0 %0 / 30
    segment.cpp +
    40.0%40.0%
    +     		40.0 %54 / 13546.2 %6 / 13
    <unnamed>40.0 %54 / 13546.2 %6 / 13
    trajectory_sampling.cpp +
    42.0%42.0%
    +     		42.0 %29 / 6928.6 %2 / 7
    <unnamed>42.0 %29 / 6928.6 %2 / 7
    polynomial.cpp +
    49.5%49.5%
    +     		49.5 %54 / 10954.5 %6 / 11
    <unnamed>49.5 %54 / 10954.5 %6 / 11
    vertex.cpp +
    50.0%50.0%
    +     		50.0 %135 / 27035.3 %6 / 17
    <unnamed>50.0 %135 / 27035.3 %6 / 17
    trajectory.cpp +
    57.3%57.3%
    +     		57.3 %180 / 31447.8 %11 / 23
    <unnamed>57.3 %180 / 31447.8 %11 / 23
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail.html new file mode 100644 index 0000000000..c0f9cd18b0 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-detail.html @@ -0,0 +1,202 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generationHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:452103643.6 %
    Date:2024-04-09 22:03:33Functions:3110529.5 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
    motion_defines.cpp +
    0.0%
    +     		0.0 %0 / 280.0 %0 / 4
    polynomial.cpp +
    49.5%49.5%
    +     		49.5 %54 / 10954.5 %6 / 11
    <unnamed>49.5 %54 / 10954.5 %6 / 11
    segment.cpp +
    40.0%40.0%
    +     		40.0 %54 / 13546.2 %6 / 13
    <unnamed>40.0 %54 / 13546.2 %6 / 13
    timing.cpp +
    0.0%
    +     		0.0 %0 / 1110.0 %0 / 30
    trajectory.cpp +
    57.3%57.3%
    +     		57.3 %180 / 31447.8 %11 / 23
    <unnamed>57.3 %180 / 31447.8 %11 / 23
    trajectory_sampling.cpp +
    42.0%42.0%
    +     		42.0 %29 / 6928.6 %2 / 7
    <unnamed>42.0 %29 / 6928.6 %2 / 7
    vertex.cpp +
    50.0%50.0%
    +     		50.0 %135 / 27035.3 %6 / 17
    <unnamed>50.0 %135 / 27035.3 %6 / 17
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-sort-f.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-sort-f.html new file mode 100644 index 0000000000..03fd115050 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-sort-f.html @@ -0,0 +1,162 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generationHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:452103643.6 %
    Date:2024-04-09 22:03:33Functions:3110529.5 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
    motion_defines.cpp +
    0.0%
    +     		0.0 %0 / 280.0 %0 / 4
    timing.cpp +
    0.0%
    +     		0.0 %0 / 1110.0 %0 / 30
    trajectory_sampling.cpp +
    42.0%42.0%
    +     		42.0 %29 / 6928.6 %2 / 7
    vertex.cpp +
    50.0%50.0%
    +     		50.0 %135 / 27035.3 %6 / 17
    segment.cpp +
    40.0%40.0%
    +     		40.0 %54 / 13546.2 %6 / 13
    trajectory.cpp +
    57.3%57.3%
    +     		57.3 %180 / 31447.8 %11 / 23
    polynomial.cpp +
    49.5%49.5%
    +     		49.5 %54 / 10954.5 %6 / 11
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-sort-l.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-sort-l.html new file mode 100644 index 0000000000..feaac424f4 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index-sort-l.html @@ -0,0 +1,162 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generationHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:452103643.6 %
    Date:2024-04-09 22:03:33Functions:3110529.5 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
    motion_defines.cpp +
    0.0%
    +     		0.0 %0 / 280.0 %0 / 4
    timing.cpp +
    0.0%
    +     		0.0 %0 / 1110.0 %0 / 30
    segment.cpp +
    40.0%40.0%
    +     		40.0 %54 / 13546.2 %6 / 13
    trajectory_sampling.cpp +
    42.0%42.0%
    +     		42.0 %29 / 6928.6 %2 / 7
    polynomial.cpp +
    49.5%49.5%
    +     		49.5 %54 / 10954.5 %6 / 11
    vertex.cpp +
    50.0%50.0%
    +     		50.0 %135 / 27035.3 %6 / 17
    trajectory.cpp +
    57.3%57.3%
    +     		57.3 %180 / 31447.8 %11 / 23
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index.html new file mode 100644 index 0000000000..9862169a5f --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/index.html @@ -0,0 +1,162 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generationHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:452103643.6 %
    Date:2024-04-09 22:03:33Functions:3110529.5 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
    motion_defines.cpp +
    0.0%
    +     		0.0 %0 / 280.0 %0 / 4
    polynomial.cpp +
    49.5%49.5%
    +     		49.5 %54 / 10954.5 %6 / 11
    segment.cpp +
    40.0%40.0%
    +     		40.0 %54 / 13546.2 %6 / 13
    timing.cpp +
    0.0%
    +     		0.0 %0 / 1110.0 %0 / 30
    trajectory.cpp +
    57.3%57.3%
    +     		57.3 %180 / 31447.8 %11 / 23
    trajectory_sampling.cpp +
    42.0%42.0%
    +     		42.0 %29 / 6928.6 %2 / 7
    vertex.cpp +
    50.0%50.0%
    +     		50.0 %135 / 27035.3 %6 / 17
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.func-sort-c.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.func-sort-c.html new file mode 100644 index 0000000000..679b69b7d5 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.func-sort-c.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - motion_defines.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:0280.0 %
    Date:2024-04-09 22:03:33Functions:040.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::positionDerivativeToInt(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::orientationDerivativeToInt(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::positionDerivativeToString[abi:cxx11](int)0
    eth_trajectory_generation::orintationDerivativeToString[abi:cxx11](int)0
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.func.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.func.html new file mode 100644 index 0000000000..6b963509ff --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.func.html @@ -0,0 +1,96 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - motion_defines.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:0280.0 %
    Date:2024-04-09 22:03:33Functions:040.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::positionDerivativeToInt(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::orientationDerivativeToInt(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::positionDerivativeToString[abi:cxx11](int)0
    eth_trajectory_generation::orintationDerivativeToString[abi:cxx11](int)0
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.frameset.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.frameset.html new file mode 100644 index 0000000000..ae6bbb029b --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.html new file mode 100644 index 0000000000..e143e911f8 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.html @@ -0,0 +1,174 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - motion_defines.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:0280.0 %
    Date:2024-04-09 22:03:33Functions:040.0 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #include <eth_trajectory_generation/motion_defines.h>
+      22             : 
+      23             : namespace eth_trajectory_generation
+      24             : {
+      25             : 
+      26             : /* positionDerivativeToString //{ */
+      27             : 
+      28           0 : std::string positionDerivativeToString(int derivative) {
+      29           0 :   if (derivative >= derivative_order::POSITION && derivative <= derivative_order::SNAP) {
+      30             :     static constexpr const char* text[] = {"position", "velocity", "acceleration", "jerk", "snap"};
+      31           0 :     return std::string(text[derivative]);
+      32             :   } else {
+      33           0 :     return std::string("invalid");
+      34             :   }
+      35             : }
+      36             : 
+      37             : //}
+      38             : 
+      39             : /* positionDerivativeToInt() //{ */
+      40             : 
+      41           0 : int positionDerivativeToInt(const std::string& string) {
+      42             :   using namespace derivative_order;
+      43           0 :   if (string == "position") {
+      44           0 :     return POSITION;
+      45           0 :   } else if (string == "velocity") {
+      46           0 :     return VELOCITY;
+      47           0 :   } else if (string == "acceleration") {
+      48           0 :     return ACCELERATION;
+      49           0 :   } else if (string == "jerk") {
+      50           0 :     return JERK;
+      51           0 :   } else if (string == "snap") {
+      52           0 :     return SNAP;
+      53             :   } else {
+      54           0 :     return INVALID;
+      55             :   }
+      56             : }
+      57             : 
+      58             : //}
+      59             : 
+      60             : /* orintationDerivativeToString() //{ */
+      61             : 
+      62           0 : std::string orintationDerivativeToString(int derivative) {
+      63           0 :   if (derivative >= derivative_order::ORIENTATION && derivative <= derivative_order::ANGULAR_ACCELERATION) {
+      64             :     static constexpr const char* text[] = {"orientation", "angular_velocity", "angular_acceleration"};
+      65           0 :     return std::string(text[derivative]);
+      66             :   } else {
+      67           0 :     return std::string("invalid");
+      68             :   }
+      69             : }
+      70             : 
+      71             : //}
+      72             : 
+      73             : /* orientationDerivativeToInt() //{ */
+      74             : 
+      75           0 : int orientationDerivativeToInt(const std::string& string) {
+      76             :   using namespace derivative_order;
+      77           0 :   if (string == "orientation") {
+      78           0 :     return ORIENTATION;
+      79           0 :   } else if (string == "angular_velocity") {
+      80           0 :     return ANGULAR_VELOCITY;
+      81           0 :   } else if (string == "angular_acceleration") {
+      82           0 :     return ANGULAR_ACCELERATION;
+      83             :   } else {
+      84           0 :     return INVALID;
+      85             :   }
+      86             : }
+      87             : 
+      88             : //}
+      89             : 
+      90             : }  // namespace eth_trajectory_generation
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.overview.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.overview.html new file mode 100644 index 0000000000..3819ee7329 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.overview.html @@ -0,0 +1,43 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp + + + + + + + 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/eth_trajectory_generation/motion_defines.cpp.gcov.png b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/motion_defines.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..bade438aeee28f72eefe41ba50044c764ab87b8c GIT binary patch literal 473 zcmV;~0Ve*5P)KS6jBiAPj^ZBzwXg;QqH_DG9G6Ehv6??oHK7nKO9}IqJ|&n)i5^Kd$tGv;bP4 zZ$uSHUU3(8B!+??jP~|EG+jAe#1#qR4v%Z`764Ln?`O^_v#nbmkOAJbf#2-NI;ab~0OG zG&NipuOs{(;~lDHw0hhyPSAy)JX||wZhNAdIV$vKOXVV{p + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - polynomial.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:5410949.5 %
    Date:2024-04-09 22:03:33Functions:61154.5 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::Polynomial::offsetPolynomial(double)0
    eth_trajectory_generation::Polynomial::computeMinMax(double, double, int, std::pair<double, double>*, std::pair<double, double>*) const0
    eth_trajectory_generation::Polynomial::selectMinMaxFromRoots(double, double, int, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1> const&, std::pair<double, double>*, std::pair<double, double>*) const0
    eth_trajectory_generation::Polynomial::selectMinMaxFromCandidates(std::vector<double, std::allocator<double> > const&, int, std::pair<double, double>*, std::pair<double, double>*) const0
    eth_trajectory_generation::Polynomial::getPolynomialWithAppendedCoefficients(int, eth_trajectory_generation::Polynomial*) const0
    eth_trajectory_generation::computeBaseCoefficients(int)82
    eth_trajectory_generation::Polynomial::scalePolynomialInTime(double)660
    eth_trajectory_generation::Polynomial::convolve(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)1980
    eth_trajectory_generation::Polynomial::selectMinMaxCandidatesFromRoots(double, double, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1> const&, std::vector<double, std::allocator<double> >*)2970
    eth_trajectory_generation::Polynomial::computeMinMaxCandidates(double, double, int, std::vector<double, std::allocator<double> >*) const2970
    eth_trajectory_generation::Polynomial::getRoots(int, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1>*) const2970
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.func.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.func.html new file mode 100644 index 0000000000..c60f58701a --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.func.html @@ -0,0 +1,124 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - polynomial.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:5410949.5 %
    Date:2024-04-09 22:03:33Functions:61154.5 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::Polynomial::offsetPolynomial(double)0
    eth_trajectory_generation::Polynomial::scalePolynomialInTime(double)660
    eth_trajectory_generation::Polynomial::selectMinMaxCandidatesFromRoots(double, double, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1> const&, std::vector<double, std::allocator<double> >*)2970
    eth_trajectory_generation::Polynomial::convolve(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)1980
    eth_trajectory_generation::computeBaseCoefficients(int)82
    eth_trajectory_generation::Polynomial::computeMinMax(double, double, int, std::pair<double, double>*, std::pair<double, double>*) const0
    eth_trajectory_generation::Polynomial::selectMinMaxFromRoots(double, double, int, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1> const&, std::pair<double, double>*, std::pair<double, double>*) const0
    eth_trajectory_generation::Polynomial::computeMinMaxCandidates(double, double, int, std::vector<double, std::allocator<double> >*) const2970
    eth_trajectory_generation::Polynomial::selectMinMaxFromCandidates(std::vector<double, std::allocator<double> > const&, int, std::pair<double, double>*, std::pair<double, double>*) const0
    eth_trajectory_generation::Polynomial::getPolynomialWithAppendedCoefficients(int, eth_trajectory_generation::Polynomial*) const0
    eth_trajectory_generation::Polynomial::getRoots(int, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1>*) const2970
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.frameset.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.frameset.html new file mode 100644 index 0000000000..ed014467f9 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.html new file mode 100644 index 0000000000..8386f16818 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.html @@ -0,0 +1,325 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - polynomial.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:5410949.5 %
    Date:2024-04-09 22:03:33Functions:61154.5 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      17             :  * implied. See the License for the specific language governing
+      18             :  * permissions and limitations under the License.
+      19             :  */
+      20             : 
+      21             : #include <eth_trajectory_generation/polynomial.h>
+      22             : #include <eth_trajectory_generation/rpoly/rpoly_ak1.h>
+      23             : 
+      24             : #include <algorithm>
+      25             : #include <limits>
+      26             : 
+      27             : namespace eth_trajectory_generation
+      28             : {
+      29             : 
+      30        2970 : bool Polynomial::getRoots(int derivative, Eigen::VectorXcd* roots) const {
+      31        2970 :   return findRootsJenkinsTraub(getCoefficients(derivative), roots);
+      32             : }
+      33             : 
+      34             : /* selectMinMaxCandidatesFromRoots() //{ */
+      35             : 
+      36        2970 : bool Polynomial::selectMinMaxCandidatesFromRoots(double t_start, double t_end, const Eigen::VectorXcd& roots_derivative_of_derivative,
+      37             :                                                  std::vector<double>* candidates) {
+      38        2970 :   CHECK_NOTNULL(candidates);
+      39        2970 :   if (t_start > t_end) {
+      40           0 :     LOG(WARNING) << "t_start is greater than t_end.";
+      41           0 :     return false;
+      42             :   }
+      43        2970 :   candidates->clear();
+      44        2970 :   candidates->reserve(roots_derivative_of_derivative.size() + 2);
+      45             :   // Put start and end in, as they are valid candidates.
+      46        2970 :   candidates->push_back(t_start);
+      47        2970 :   candidates->push_back(t_end);
+      48       27072 :   for (size_t i = 0; i < static_cast<size_t>(roots_derivative_of_derivative.size()); i++) {
+      49             :     // Only real roots are considered as critical points.
+      50       24102 :     if (std::abs(roots_derivative_of_derivative[i].imag()) > std::numeric_limits<double>::epsilon()) {
+      51       16676 :       continue;
+      52             :     }
+      53       11388 :     const double candidate = roots_derivative_of_derivative[i].real();
+      54             : 
+      55             :     // Do not evaluate points outside the domain.
+      56       11388 :     if (candidate < t_start || candidate > t_end) {
+      57        3962 :       continue;
+      58             :     } else {
+      59        7426 :       candidates->push_back(candidate);
+      60             :     }
+      61             :   }
+      62        2970 :   return true;
+      63             : }
+      64             : 
+      65             : //}
+      66             : 
+      67             : /* computeMinMaxCandidates() //{ */
+      68             : 
+      69        2970 : bool Polynomial::computeMinMaxCandidates(double t_start, double t_end, int derivative, std::vector<double>* candidates) const {
+      70        2970 :   CHECK_NOTNULL(candidates);
+      71        2970 :   candidates->clear();
+      72        2970 :   if (N_ - derivative - 1 < 0) {
+      73           0 :     LOG(WARNING) << "N - derivative - 1 has to be at least 0.";
+      74           0 :     return false;
+      75             :   }
+      76        5940 :   Eigen::VectorXcd roots;
+      77        2970 :   bool             success = getRoots(derivative + 1, &roots);
+      78        2970 :   if (!success) {
+      79           0 :     VLOG(1) << "Couldn't find roots, polynomial may be constant.";
+      80             :   }
+      81        2970 :   if (!selectMinMaxCandidatesFromRoots(t_start, t_end, roots, candidates)) {
+      82           0 :     return false;
+      83             :   }
+      84        2970 :   return true;
+      85             : }
+      86             : 
+      87             : //}
+      88             : 
+      89             : /* selectMinMaxFromRoots() //{ */
+      90             : 
+      91           0 : bool Polynomial::selectMinMaxFromRoots(double t_start, double t_end, int derivative, const Eigen::VectorXcd& roots_derivative_of_derivative,
+      92             :                                        std::pair<double, double>* minimum, std::pair<double, double>* maximum) const {
+      93           0 :   CHECK_NOTNULL(minimum);
+      94           0 :   CHECK_NOTNULL(maximum);
+      95             :   // Find candidates in interval t_start to t_end computing the roots.
+      96           0 :   std::vector<double> candidates;
+      97           0 :   if (!selectMinMaxCandidatesFromRoots(t_start, t_end, roots_derivative_of_derivative, &candidates)) {
+      98           0 :     return false;
+      99             :   }
+     100             :   // Evaluate minimum and maximum.
+     101           0 :   return selectMinMaxFromCandidates(candidates, derivative, minimum, maximum);
+     102             : }
+     103             : 
+     104             : //}
+     105             : 
+     106             : /* computeMinMax() //{ */
+     107             : 
+     108           0 : bool Polynomial::computeMinMax(double t_start, double t_end, int derivative, std::pair<double, double>* minimum, std::pair<double, double>* maximum) const {
+     109           0 :   CHECK_NOTNULL(minimum);
+     110           0 :   CHECK_NOTNULL(maximum);
+     111             :   // Find candidates in interval t_start to t_end by computing the roots.
+     112           0 :   std::vector<double> candidates;
+     113           0 :   if (!computeMinMaxCandidates(t_start, t_end, derivative, &candidates)) {
+     114           0 :     return false;
+     115             :   }
+     116             :   // Evaluate minimum and maximum.
+     117           0 :   return selectMinMaxFromCandidates(candidates, derivative, minimum, maximum);
+     118             : }
+     119             : 
+     120             : //}
+     121             : 
+     122             : /* selectMinMaxFromCandidates() //{ */
+     123             : 
+     124           0 : bool Polynomial::selectMinMaxFromCandidates(const std::vector<double>& candidates, int derivative, std::pair<double, double>* minimum,
+     125             :                                             std::pair<double, double>* maximum) const {
+     126           0 :   CHECK_NOTNULL(minimum);
+     127           0 :   CHECK_NOTNULL(maximum);
+     128           0 :   if (candidates.empty()) {
+     129           0 :     LOG(WARNING) << "Cannot find extrema from an empty candidates vector.";
+     130           0 :     return false;
+     131             :   }
+     132           0 :   minimum->first  = candidates[0];
+     133           0 :   minimum->second = std::numeric_limits<double>::max();
+     134           0 :   maximum->first  = candidates[0];
+     135           0 :   maximum->second = std::numeric_limits<double>::lowest();
+     136             : 
+     137           0 :   for (const double& t : candidates) {
+     138           0 :     const double value = evaluate(t, derivative);
+     139           0 :     if (value < minimum->second) {
+     140           0 :       minimum->first  = t;
+     141           0 :       minimum->second = value;
+     142             :     }
+     143           0 :     if (value > maximum->second) {
+     144           0 :       maximum->first  = t;
+     145           0 :       maximum->second = value;
+     146             :     }
+     147             :   }
+     148           0 :   return true;
+     149             : }
+     150             : 
+     151             : //}
+     152             : 
+     153             : /* computeBaseCoefficients() //{ */
+     154             : 
+     155          82 : Eigen::MatrixXd computeBaseCoefficients(int N) {
+     156          82 :   Eigen::MatrixXd base_coefficients(N, N);
+     157             : 
+     158          82 :   base_coefficients.setZero();
+     159          82 :   base_coefficients.row(0).setOnes();
+     160             : 
+     161          82 :   const int DEG   = N - 1;
+     162          82 :   int       order = DEG;
+     163        1804 :   for (int n = 1; n < N; n++) {
+     164       22386 :     for (int i = DEG - order; i < N; i++) {
+     165       20664 :       base_coefficients(n, i) = (order - DEG + i) * base_coefficients(n - 1, i);
+     166             :     }
+     167        1722 :     order--;
+     168             :   }
+     169          82 :   return base_coefficients;
+     170             : }
+     171             : 
+     172             : //}
+     173             : 
+     174             : /* convolve() //{ */
+     175             : 
+     176        1980 : Eigen::VectorXd Polynomial::convolve(const Eigen::VectorXd& data, const Eigen::VectorXd& kernel) {
+     177        1980 :   const int       convolution_dimension = getConvolutionLength(data.size(), kernel.size());
+     178        1980 :   Eigen::VectorXd convolved             = Eigen::VectorXd::Zero(convolution_dimension);
+     179        3960 :   Eigen::VectorXd kernel_reverse        = kernel.reverse();
+     180             : 
+     181       29700 :   for (int i = 0; i < convolution_dimension; i++) {
+     182       27720 :     const int data_idx = i - kernel.size() + 1;
+     183             : 
+     184       27720 :     int lower_bound = std::max(0, -data_idx);
+     185       27720 :     int upper_bound = std::min(kernel.size(), data.size() - data_idx);
+     186             : 
+     187      139920 :     for (int kernel_idx = lower_bound; kernel_idx < upper_bound; ++kernel_idx) {
+     188      112200 :       convolved[i] += kernel_reverse[kernel_idx] * data[data_idx + kernel_idx];
+     189             :     }
+     190             :   }
+     191        3960 :   return convolved;
+     192             : }
+     193             : 
+     194             : //}
+     195             : 
+     196             : /* getPolynomialWithAppendedCoefficients() //{ */
+     197             : 
+     198           0 : bool Polynomial::getPolynomialWithAppendedCoefficients(int new_N, Polynomial* new_polynomial) const {
+     199           0 :   if (new_N == N_) {
+     200           0 :     *new_polynomial = *this;
+     201           0 :     return true;
+     202           0 :   } else if (new_N < N_) {
+     203           0 :     LOG(WARNING) << "You shan't decrease the number of coefficients.";
+     204           0 :     *new_polynomial = *this;
+     205           0 :     return false;
+     206             :   } else {
+     207           0 :     Eigen::VectorXd coeffs = Eigen::VectorXd::Zero(new_N);
+     208           0 :     coeffs.head(N_)        = coefficients_;
+     209           0 :     *new_polynomial        = Polynomial(coeffs);
+     210           0 :     return true;
+     211             :   }
+     212             : }
+     213             : 
+     214             : //}
+     215             : 
+     216             : /* scalePolynomialInTime() //{ */
+     217             : 
+     218         660 : void Polynomial::scalePolynomialInTime(double scaling_factor) {
+     219         660 :   double scale = 1.0;
+     220        7260 :   for (int n = 0; n < N_; n++) {
+     221        6600 :     coefficients_[n] *= scale;
+     222        6600 :     scale *= scaling_factor;
+     223             :   }
+     224         660 : }
+     225             : 
+     226             : //}
+     227             : 
+     228             : /* offsetPolynomial() //{ */
+     229             : 
+     230           0 : void Polynomial::offsetPolynomial(const double offset) {
+     231           0 :   if (coefficients_.size() == 0)
+     232           0 :     return;
+     233             : 
+     234           0 :   coefficients_[0] += offset;
+     235             : }
+     236             : 
+     237             : //}
+     238             : 
+     239             : Eigen::MatrixXd Polynomial::base_coefficients_ = computeBaseCoefficients(Polynomial::kMaxConvolutionSize);
+     240             : 
+     241             : }  // namespace eth_trajectory_generation
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.overview.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.overview.html new file mode 100644 index 0000000000..a9279c2726 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.overview.html @@ -0,0 +1,81 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.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 + + +
    + Top

    + Overview +
    + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.png b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/polynomial.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..3586e7bbfd9d9d17fb06eb67308a9f87998562f1 GIT binary patch literal 1168 zcmV;B1aJF^P)0{{R3u754n0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vpH9 zt-16g87txq;1iDCdK|6o4mKVvuxCpL#~_}2&3F+MXpry~-Nf^t;K6B}yhtHeY8?Zj zuyX`9Eb%)ciEd5a1153vy+O^7WE^{(Q+g9xPFGSad-oh)D0}OluG`8Y3e+j2SrLVUC-rdxC8(reL_%_Rfk`1XP(H8o zJZ&k3Q>dGvD85-puqfj$aDLxV>rMI}aL!toq(bsRft zXBS?GN0!g4{gFYJ!ep9}9WKo#SJn^0?7hHU60|_Iaq6Ij@4p(4STZqSeie@~7V(nN zuRLe|^LWIexwG5zfZBzW>=PZ?Dx_NL4DRabOMx4J17tox;^=5D?0QJb^OblcWi%P? z4tOROZW&<-$Lyw#3!5A%jg7%%?f`Yjc#~#*X*B?vRKhktFJshuD5K8w9s%ck_z4-O zr#KAodyd~Lduyh+IJG5xeKk^rc_u;1R#!g1{ZI_{)4-!QP(4~&!AHROI0nK}e&^$c zBW!5cnGNRA!HfobDISSCC_Q+-^CA7NA>5>*2i(oic913}DGQvOrShHO!A&0;&K9&1 zfttxBHwnA!k;`q9!djFG1+%2 zfcA`l4LRDthFVVyKA{N!*S~lD!nR_2bDt_(EcsM>Y;_wKm+bK|diTTPL(o;y0Y?#D zMvZU}+L`J4IEfRl`g&ET(x{JN>dkW|1)BcEhc6%|l4~vp(=8IeqtOR`Vu~aG6%V=c i$v4lDZ@l8z68jGdKzV+`R@Ni{0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpolyHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:36439891.5 %
    Date:2024-04-09 22:03:33Functions:1212100.0 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
    rpoly_ak1.cpp +
    91.5%91.5%
    +     		91.5 %364 / 398100.0 %12 / 12
    <unnamed>91.5 %364 / 398100.0 %12 / 12
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail-sort-l.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail-sort-l.html new file mode 100644 index 0000000000..e9a5b39bf2 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail-sort-l.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpolyHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:36439891.5 %
    Date:2024-04-09 22:03:33Functions:1212100.0 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
    rpoly_ak1.cpp +
    91.5%91.5%
    +     		91.5 %364 / 398100.0 %12 / 12
    <unnamed>91.5 %364 / 398100.0 %12 / 12
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail.html new file mode 100644 index 0000000000..ad7afe8acc --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-detail.html @@ -0,0 +1,110 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpolyHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:36439891.5 %
    Date:2024-04-09 22:03:33Functions:1212100.0 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( hide details ) Sort by line coverageFunctions Sort by function coverage
    rpoly_ak1.cpp +
    91.5%91.5%
    +     		91.5 %364 / 398100.0 %12 / 12
    <unnamed>91.5 %364 / 398100.0 %12 / 12
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-sort-f.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-sort-f.html new file mode 100644 index 0000000000..210606e679 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-sort-f.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpolyHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:36439891.5 %
    Date:2024-04-09 22:03:33Functions:1212100.0 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
    rpoly_ak1.cpp +
    91.5%91.5%
    +     		91.5 %364 / 398100.0 %12 / 12
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-sort-l.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-sort-l.html new file mode 100644 index 0000000000..a0df9607af --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index-sort-l.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpolyHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:36439891.5 %
    Date:2024-04-09 22:03:33Functions:1212100.0 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
    rpoly_ak1.cpp +
    91.5%91.5%
    +     		91.5 %364 / 398100.0 %12 / 12
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index.html new file mode 100644 index 0000000000..720f2e4717 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/index.html @@ -0,0 +1,102 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpolyHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:36439891.5 %
    Date:2024-04-09 22:03:33Functions:1212100.0 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + +

    Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
    rpoly_ak1.cpp +
    91.5%91.5%
    +     		91.5 %364 / 398100.0 %12 / 12
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.func-sort-c.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.func-sort-c.html new file mode 100644 index 0000000000..09ad577692 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.func-sort-c.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly - rpoly_ak1.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:36439891.5 %
    Date:2024-04-09 22:03:33Functions:1212100.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::rpoly_impl::rpoly_ak1(double*, int*, double*, double*)2862
    eth_trajectory_generation::rpolyWrapper(double*, int*, double*, double*)2862
    eth_trajectory_generation::findLastNonZeroCoeff(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)2970
    eth_trajectory_generation::findRootsJenkinsTraub(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1>*)2970
    eth_trajectory_generation::rpoly_impl::QuadIT_ak1(int, int*, double, double, double*, double*, double*, double*, double*, int, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*)8201
    eth_trajectory_generation::rpoly_impl::RealIT_ak1(int*, int*, double*, int, double*, int, double*, double*, double*, double*, double*)8509
    eth_trajectory_generation::rpoly_impl::Fxshfr_ak1(int, int*, double, double, double*, int, double*, int, double*, double*, double*, double*, double*)13357
    eth_trajectory_generation::rpoly_impl::Quad_ak1(double, double, double, double*, double*, double*, double*)26711
    eth_trajectory_generation::rpoly_impl::newest_ak1(int, double*, double*, double, double, double, double, double, double, double, double, double, double, double, double, double*, int, double*)48626
    eth_trajectory_generation::rpoly_impl::nextK_ak1(int, int, double, double, double, double*, double*, double*, double*, double*)48646
    eth_trajectory_generation::rpoly_impl::calcSC_ak1(int, double, double, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double, double, double*)80295
    eth_trajectory_generation::rpoly_impl::QuadSD_ak1(int, double, double, double*, double*, double*, double*)117836
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.func.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.func.html new file mode 100644 index 0000000000..d750fa6813 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.func.html @@ -0,0 +1,128 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly - rpoly_ak1.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:36439891.5 %
    Date:2024-04-09 22:03:33Functions:1212100.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::rpoly_impl::Fxshfr_ak1(int, int*, double, double, double*, int, double*, int, double*, double*, double*, double*, double*)13357
    eth_trajectory_generation::rpoly_impl::QuadIT_ak1(int, int*, double, double, double*, double*, double*, double*, double*, int, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*)8201
    eth_trajectory_generation::rpoly_impl::QuadSD_ak1(int, double, double, double*, double*, double*, double*)117836
    eth_trajectory_generation::rpoly_impl::RealIT_ak1(int*, int*, double*, int, double*, int, double*, double*, double*, double*, double*)8509
    eth_trajectory_generation::rpoly_impl::calcSC_ak1(int, double, double, double*, double*, double*, double*, double*, double*, double*, double*, double*, double*, double, double, double*)80295
    eth_trajectory_generation::rpoly_impl::newest_ak1(int, double*, double*, double, double, double, double, double, double, double, double, double, double, double, double, double*, int, double*)48626
    eth_trajectory_generation::rpoly_impl::Quad_ak1(double, double, double, double*, double*, double*, double*)26711
    eth_trajectory_generation::rpoly_impl::nextK_ak1(int, int, double, double, double, double*, double*, double*, double*, double*)48646
    eth_trajectory_generation::rpoly_impl::rpoly_ak1(double*, int*, double*, double*)2862
    eth_trajectory_generation::rpolyWrapper(double*, int*, double*, double*)2862
    eth_trajectory_generation::findLastNonZeroCoeff(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)2970
    eth_trajectory_generation::findRootsJenkinsTraub(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1>*)2970
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.frameset.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.frameset.html new file mode 100644 index 0000000000..79860f19eb --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.html new file mode 100644 index 0000000000..5dcebb34cf --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.html @@ -0,0 +1,1027 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly - rpoly_ak1.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:36439891.5 %
    Date:2024-04-09 22:03:33Functions:1212100.0 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : // rpoly_ak1.cpp - Program for calculating the roots of a polynomial of real
+       2             : // coefficients.
+       3             : // Written in Microsoft Visual Studio Express 2013 for Windows Desktop
+       4             : // 27 May 2014
+       5             : //
+       6             : // The sub-routines listed below are translations of the FORTRAN routines
+       7             : // included in RPOLY.FOR,
+       8             : // posted off the NETLIB site as TOMS/493:
+       9             : //
+      10             : // http://www.netlib.org/toms/493
+      11             : //
+      12             : // TOMS/493 is based on the Jenkins-Traub algorithm.
+      13             : //
+      14             : // To distinguish the routines posted below from others, an _ak1 suffix has been
+      15             : // appended to them.
+      16             : //
+      17             : // Following is a list of the major changes made in the course of translating
+      18             : // the TOMS/493 routines
+      19             : // to the C++ versions posted below:
+      20             : // 1) All global variables have been eliminated.
+      21             : // 2) The "FAIL" parameter passed into RPOLY.FOR has been eliminated.
+      22             : // 3) RPOLY.FOR solves polynomials of degree up to 100, but does not explicitly
+      23             : // state this limit.
+      24             : //     rpoly_ak1 explicitly states this limit; uses the macro name MAXDEGREE to
+      25             : //     specify this limit;
+      26             : //     and does a check to ensure that the user input variable Degree is not
+      27             : //     greater than MAXDEGREE
+      28             : //     (if it is, an error message is output and rpoly_ak1 terminates). If a
+      29             : //     user wishes to compute
+      30             : //     roots of polynomials of degree greater than MAXDEGREE, using a macro name
+      31             : //     like MAXDEGREE provides
+      32             : //     the simplest way of offering this capability.
+      33             : // 4) All "GO TO" statements have been eliminated.
+      34             : //
+      35             : // A small main program is included also, to provide an example of how to use
+      36             : // rpoly_ak1. In this
+      37             : // example, data is input from a file to eliminate the need for a user to type
+      38             : // data in via
+      39             : // the console.
+      40             : 
+      41             : #include <iostream>
+      42             : #include <fstream>
+      43             : #include <cctype>
+      44             : #include <cmath>
+      45             : #include <cfloat>
+      46             : 
+      47             : #include <eth_trajectory_generation/rpoly/rpoly_ak1.h>
+      48             : 
+      49             : namespace eth_trajectory_generation
+      50             : {
+      51             : 
+      52             : constexpr int kRpolyMaxDegree = 100;
+      53             : 
+      54             : // Wraps the call to rpoly_ak1.
+      55             : void rpolyWrapper(double* coefficients_decreasing, int* degree, double* roots_real, double* roots_imag);
+      56             : 
+      57             : /* findLastNonZeroCoeff() //{ */
+      58             : 
+      59        2970 : int findLastNonZeroCoeff(const Eigen::VectorXd& coefficients) {
+      60        2970 :   int last_non_zero_coefficient = -1;
+      61             : 
+      62             :   // Find last non-zero coefficient:
+      63        9666 :   for (int i = coefficients.size() - 1; i != -1; i--) {
+      64        9558 :     if (std::abs(coefficients(i)) >= std::numeric_limits<double>::min()) {
+      65        2862 :       last_non_zero_coefficient = i;
+      66        2862 :       break;
+      67             :     }
+      68             :   }
+      69        2970 :   return last_non_zero_coefficient;
+      70             : }
+      71             : 
+      72             : //}
+      73             : 
+      74             : /* findRootsJenkinsTraub() //{ */
+      75             : 
+      76        2970 : bool findRootsJenkinsTraub(const Eigen::VectorXd& coefficients_increasing, Eigen::VectorXcd* roots) {
+      77             :   // Remove trailing zeros.
+      78        2970 :   const int last_non_zero_coefficient = findLastNonZeroCoeff(coefficients_increasing);
+      79        2970 :   if (last_non_zero_coefficient == -1) {
+      80             :     // The polynomial has all zero coefficients and has no roots.
+      81         108 :     roots->resize(0);
+      82         108 :     return true;
+      83             :   }
+      84             : 
+      85             :   // Reverse coefficients in descending order.
+      86        5724 :   Eigen::VectorXd coefficients_decreasing = coefficients_increasing.head(last_non_zero_coefficient + 1).reverse();
+      87             : 
+      88        2862 :   const int n_coefficients = coefficients_decreasing.size();
+      89        2862 :   if (n_coefficients < 2) {
+      90             :     // The polynomial is 0th order and has no roots.
+      91           0 :     roots->resize(0);
+      92           0 :     return true;
+      93             :   }
+      94        2862 :   int     degree     = n_coefficients - 1;
+      95        2862 :   double* polynomial = new double[kRpolyMaxDegree + 1];
+      96        2862 :   double* roots_real = new double[kRpolyMaxDegree];
+      97        2862 :   double* roots_imag = new double[kRpolyMaxDegree];
+      98             : 
+      99       29826 :   for (size_t i = 0; i < n_coefficients; i++) {
+     100       26964 :     polynomial[i] = coefficients_decreasing(i);
+     101             :   }
+     102             : 
+     103        2862 :   rpolyWrapper(polynomial, &degree, roots_real, roots_imag);
+     104        2862 :   if (degree > 0) {
+     105        2862 :     roots->resize(degree);
+     106       26964 :     for (int i = 0; i < degree; ++i) {
+     107       24102 :       (*roots)[i] = std::complex<double>(roots_real[i], roots_imag[i]);
+     108             :     }
+     109             :   }
+     110             : 
+     111        2862 :   delete[] polynomial;
+     112        2862 :   delete[] roots_real;
+     113        2862 :   delete[] roots_imag;
+     114             : 
+     115        2862 :   if (degree > 0) {
+     116        2862 :     return true;
+     117             :   } else {
+     118           0 :     return false;
+     119             :   }
+     120             : }
+     121             : 
+     122             : //}
+     123             : 
+     124             : /* findRootsJenkinsTraub() //{ */
+     125             : 
+     126             : namespace rpoly_impl
+     127             : {
+     128             : 
+     129             : using namespace std;
+     130             : 
+     131             : #define MAXDEGREE 100
+     132             : #define MDP1 MAXDEGREE + 1
+     133             : 
+     134             : void rpoly_ak1(double op[MDP1], int* Degree, double zeror[MAXDEGREE], double zeroi[MAXDEGREE]);
+     135             : void Fxshfr_ak1(int L2, int* NZ, double sr, double bnd, double K[MDP1], int N, double p[MDP1], int NN, double qp[MDP1], double* lzi, double* lzr, double* szi,
+     136             :                 double* szr);
+     137             : void QuadSD_ak1(int NN, double u, double v, double p[MDP1], double q[MDP1], double* a, double* b);
+     138             : int  calcSC_ak1(int N, double a, double b, double* a1, double* a3, double* a7, double* c, double* d, double* e, double* f, double* g, double* h, double K[MDP1],
+     139             :                 double u, double v, double qk[MDP1]);
+     140             : void nextK_ak1(int N, int tFlag, double a, double b, double a1, double* a3, double* a7, double K[MDP1], double qk[MDP1], double qp[MDP1]);
+     141             : void newest_ak1(int tFlag, double* uu, double* vv, double a, double a1, double a3, double a7, double b, double c, double d, double f, double g, double h,
+     142             :                 double u, double v, double K[MDP1], int N, double p[MDP1]);
+     143             : void QuadIT_ak1(int N, int* NZ, double uu, double vv, double* szr, double* szi, double* lzr, double* lzi, double qp[MDP1], int NN, double* a, double* b,
+     144             :                 double p[MDP1], double qk[MDP1], double* a1, double* a3, double* a7, double* d, double* e, double* f, double* g, double* h, double K[MDP1]);
+     145             : void RealIT_ak1(int* iFlag, int* NZ, double* sss, int N, double p[MDP1], int NN, double qp[MDP1], double* szr, double* szi, double K[MDP1], double qk[MDP1]);
+     146             : void Quad_ak1(double a, double b1, double c, double* sr, double* si, double* lr, double* li);
+     147             : 
+     148        2862 : void rpoly_ak1(double op[MDP1], int* Degree, double zeror[MAXDEGREE], double zeroi[MAXDEGREE]) {
+     149             :   int i, j, jj, l, N, NM1, NN, NZ, zerok;
+     150             : 
+     151             :   double K[MDP1], p[MDP1], pt[MDP1], qp[MDP1], temp[MDP1];
+     152             :   double bnd, df, dx, factor, ff, moduli_max, moduli_min, sc, x, xm;
+     153             :   double aa, bb, cc, lzi, lzr, sr, szi, szr, t, xx, xxx, yy;
+     154             : 
+     155        2862 :   const double RADFAC = 3.14159265358979323846 / 180;  // Degrees-to-radians conversion factor = pi/180
+     156        2862 :   const double lb2    = log(2.0);                      // Dummy variable to avoid re-calculating this value in loop below
+     157        2862 :   const double lo     = FLT_MIN / DBL_EPSILON;
+     158        2862 :   const double cosr   = cos(94.0 * RADFAC);  // = -0.069756474
+     159        2862 :   const double sinr   = sin(94.0 * RADFAC);  // = 0.99756405
+     160             : 
+     161        2862 :   if ((*Degree) > MAXDEGREE) {
+     162             :     cout << "\nThe entered Degree is greater than MAXDEGREE. Exiting rpoly. No "
+     163           0 :             "further action taken.\n";
+     164           0 :     *Degree = -1;
+     165           0 :     return;
+     166             :   }  // End ((*Degree) > MAXDEGREE)
+     167             : 
+     168             :   // Do a quick check to see if leading coefficient is 0
+     169        2862 :   if (op[0] != 0) {
+     170        2862 :     N  = *Degree;
+     171        2862 :     xx = sqrt(0.5);  // = 0.70710678
+     172        2862 :     yy = -xx;
+     173             : 
+     174             :     // Remove zeros at the origin, if any
+     175        2862 :     j = 0;
+     176        2962 :     while (op[N] == 0) {
+     177         100 :       zeror[j] = zeroi[j] = 0.0;
+     178         100 :       N--;
+     179         100 :       j++;
+     180             :     }  // End while (op[N] == 0)
+     181             : 
+     182        2862 :     NN = N + 1;
+     183             : 
+     184             :     // Make a copy of the coefficients
+     185       29726 :     for (i = 0; i < NN; i++)
+     186       26864 :       p[i] = op[i];
+     187             : 
+     188       16190 :     while (N >= 1) {  // Main loop
+     189             :       // Start the algorithm for one zero
+     190       16190 :       if (N <= 2) {
+     191             :         // Calculate the final zero or pair of zeros
+     192        2862 :         if (N < 2) {
+     193         938 :           zeror[(*Degree) - 1] = -(p[1] / p[0]);
+     194         938 :           zeroi[(*Degree) - 1] = 0.0;
+     195             :         }       // End if (N < 2)
+     196             :         else {  // else N == 2
+     197        1924 :           Quad_ak1(p[0], p[1], p[2], &zeror[(*Degree) - 2], &zeroi[(*Degree) - 2], &zeror[(*Degree) - 1], &zeroi[(*Degree) - 1]);
+     198             :         }  // End else N == 2
+     199        2862 :         break;
+     200             :       }  // End if (N <= 2)
+     201             : 
+     202             :       // Find the largest and smallest moduli of the coefficients
+     203             : 
+     204       13328 :       moduli_max = 0.0;
+     205       13328 :       moduli_min = FLT_MAX;
+     206             : 
+     207      116950 :       for (i = 0; i < NN; i++) {
+     208      103622 :         x = fabs(p[i]);
+     209      103622 :         if (x > moduli_max)
+     210       51149 :           moduli_max = x;
+     211      103622 :         if ((x != 0) && (x < moduli_min))
+     212       40262 :           moduli_min = x;
+     213             :       }  // End for i
+     214             : 
+     215             :       // Scale if there are large or very small coefficients
+     216             :       // Computes a scale factor to multiply the coefficients of the polynomial.
+     217             :       // The scaling
+     218             :       // is done to avoid overflow and to avoid undetected underflow interfering
+     219             :       // with the
+     220             :       // convergence criterion.
+     221             :       // The factor is a power of the base.
+     222             : 
+     223       13328 :       sc = lo / moduli_min;
+     224             : 
+     225       13328 :       if (((sc <= 1.0) && (moduli_max >= 10)) || ((sc > 1.0) && (FLT_MAX / sc >= moduli_max))) {
+     226        2009 :         sc     = ((sc == 0) ? FLT_MIN : sc);
+     227        2009 :         l      = (int)(log(sc) / lb2 + 0.5);
+     228        2009 :         factor = pow(2.0, l);
+     229        2009 :         if (factor != 1.0) {
+     230       22899 :           for (i = 0; i < NN; i++)
+     231       20892 :             p[i] *= factor;
+     232             :         }  // End if (factor != 1.0)
+     233             :       }    // End if (((sc <= 1.0) && (moduli_max >= 10)) || ((sc > 1.0) &&
+     234             :            // (FLT_MAX/sc >= moduli_max)))
+     235             : 
+     236             :       // Compute lower bound on moduli of zeros
+     237             : 
+     238      116950 :       for (i = 0; i < NN; i++)
+     239      103622 :         pt[i] = fabs(p[i]);
+     240       13328 :       pt[N] = -(pt[N]);
+     241             : 
+     242       13328 :       NM1 = N - 1;
+     243             : 
+     244             :       // Compute upper estimate of bound
+     245             : 
+     246       13328 :       x = exp((log(-pt[N]) - log(pt[0])) / (double)N);
+     247             : 
+     248       13328 :       if (pt[NM1] != 0) {
+     249             :         // If Newton step at the origin is better, use it
+     250       13328 :         xm = -pt[N] / pt[NM1];
+     251       13328 :         x  = ((xm < x) ? xm : x);
+     252             :       }  // End if (pt[NM1] != 0)
+     253             : 
+     254             :       // Chop the interval (0, x) until ff <= 0
+     255             : 
+     256       13328 :       xm = x;
+     257         542 :       do {
+     258       13870 :         x  = xm;
+     259       13870 :         xm = 0.1 * x;
+     260       13870 :         ff = pt[0];
+     261      110340 :         for (i = 1; i < NN; i++)
+     262       96470 :           ff = ff * xm + pt[i];
+     263       13870 :       } while (ff > 0);  // End do-while loop
+     264             : 
+     265       13328 :       dx = x;
+     266             : 
+     267             :       // Do Newton iteration until x converges to two decimal places
+     268             : 
+     269       58623 :       while (fabs(dx / x) > 0.005) {
+     270       45295 :         df = ff = pt[0];
+     271      327069 :         for (i = 1; i < N; i++) {
+     272      281774 :           ff = x * ff + pt[i];
+     273      281774 :           df = x * df + ff;
+     274             :         }  // End for i
+     275       45295 :         ff = x * ff + pt[N];
+     276       45295 :         dx = ff / df;
+     277       45295 :         x -= dx;
+     278             :       }  // End while loop
+     279             : 
+     280       13328 :       bnd = x;
+     281             : 
+     282             :       // Compute the derivative as the initial K polynomial and do 5 steps with
+     283             :       // no shift
+     284             : 
+     285       90294 :       for (i = 1; i < N; i++)
+     286       76966 :         K[i] = (double)(N - i) * p[i] / ((double)N);
+     287       13328 :       K[0] = p[0];
+     288             : 
+     289       13328 :       aa    = p[N];
+     290       13328 :       bb    = p[NM1];
+     291       13328 :       zerok = ((K[NM1] == 0) ? 1 : 0);
+     292             : 
+     293       79968 :       for (jj = 0; jj < 5; jj++) {
+     294       66640 :         cc = K[NM1];
+     295       66640 :         if (zerok) {
+     296             :           // Use unscaled form of recurrence
+     297           0 :           for (i = 0; i < NM1; i++) {
+     298           0 :             j    = NM1 - i;
+     299           0 :             K[j] = K[j - 1];
+     300             :           }  // End for i
+     301           0 :           K[0]  = 0;
+     302           0 :           zerok = ((K[NM1] == 0) ? 1 : 0);
+     303             :         }  // End if (zerok)
+     304             : 
+     305             :         else {  // else !zerok
+     306             :           // Used scaled form of recurrence if value of K at 0 is nonzero
+     307       66640 :           t = -aa / cc;
+     308      451470 :           for (i = 0; i < NM1; i++) {
+     309      384830 :             j    = NM1 - i;
+     310      384830 :             K[j] = t * K[j - 1] + p[j];
+     311             :           }  // End for i
+     312       66640 :           K[0]  = p[0];
+     313       66640 :           zerok = ((fabs(K[NM1]) <= fabs(bb) * DBL_EPSILON * 10.0) ? 1 : 0);
+     314             :         }  // End else !zerok
+     315             : 
+     316             :       }  // End for jj
+     317             : 
+     318             :       // Save K for restarts with new shifts
+     319      103622 :       for (i = 0; i < N; i++)
+     320       90294 :         temp[i] = K[i];
+     321             : 
+     322             :       // Loop to select the quadratic corresponding to each new shift
+     323             : 
+     324       13357 :       for (jj = 1; jj <= 20; jj++) {
+     325             :         // Quadratic corresponds to a double shift to a non-real point and its
+     326             :         // complex conjugate. The point has modulus BND and amplitude rotated
+     327             :         // by 94 degrees from the previous shift.
+     328             : 
+     329       13357 :         xxx = -(sinr * yy) + cosr * xx;
+     330       13357 :         yy  = sinr * xx + cosr * yy;
+     331       13357 :         xx  = xxx;
+     332       13357 :         sr  = bnd * xx;
+     333             : 
+     334             :         // Second stage calculation, fixed quadratic
+     335             : 
+     336       13357 :         Fxshfr_ak1(20 * jj, &NZ, sr, bnd, K, N, p, NN, qp, &lzi, &lzr, &szi, &szr);
+     337             : 
+     338       13357 :         if (NZ != 0) {
+     339             :           // The second stage jumps directly to one of the third stage
+     340             :           // iterations and
+     341             :           // returns here if successful. Deflate the polynomial, store the zero
+     342             :           // or
+     343             :           // zeros, and return to the main algorithm.
+     344             : 
+     345       13328 :           j        = (*Degree) - N;
+     346       13328 :           zeror[j] = szr;
+     347       13328 :           zeroi[j] = szi;
+     348       13328 :           NN       = NN - NZ;
+     349       13328 :           N        = NN - 1;
+     350       97734 :           for (i = 0; i < NN; i++)
+     351       84406 :             p[i] = qp[i];
+     352       13328 :           if (NZ != 1) {
+     353        5888 :             zeror[j + 1] = lzr;
+     354        5888 :             zeroi[j + 1] = lzi;
+     355             :           }  // End if (NZ != 1)
+     356       13328 :           break;
+     357             :         }       // End if (NZ != 0)
+     358             :         else {  // Else (NZ == 0)
+     359             : 
+     360             :           // If the iteration is unsuccessful, another quadratic is chosen after
+     361             :           // restoring K
+     362         360 :           for (i = 0; i < N; i++)
+     363         331 :             K[i] = temp[i];
+     364             :         }  // End else (NZ == 0)
+     365             : 
+     366             :       }  // End for jj
+     367             : 
+     368             :       // Return with failure if no convergence with 20 shifts
+     369             : 
+     370       13328 :       if (jj > 20) {
+     371             :         cout << "\nFailure. No convergence after 20 shifts. Program "
+     372           0 :                 "terminated.\n";
+     373           0 :         *Degree -= N;
+     374           0 :         break;
+     375             :       }  // End if (jj > 20)
+     376             : 
+     377             :     }  // End while (N >= 1)
+     378             : 
+     379             :   }       // End if op[0] != 0
+     380             :   else {  // else op[0] == 0
+     381             :     cout << "\nThe leading coefficient is zero. No further action taken. "
+     382           0 :             "Program terminated.\n";
+     383           0 :     *Degree = 0;
+     384             :   }  // End else op[0] == 0
+     385             : 
+     386        2862 :   return;
+     387             : }  // End rpoly_ak1
+     388             : 
+     389       13357 : void Fxshfr_ak1(int L2, int* NZ, double sr, double bnd, double K[MDP1], int N, double p[MDP1], int NN, double qp[MDP1], double* lzi, double* lzr, double* szi,
+     390             :                 double* szr) {
+     391             :   // Computes up to L2 fixed shift K-polynomials, testing for convergence in the
+     392             :   // linear or
+     393             :   // quadratic case. Initiates one of the variable shift iterations and returns
+     394             :   // with the
+     395             :   // number of zeros found.
+     396             : 
+     397             :   // L2 limit of fixed shift steps
+     398             :   // NZ number of zeros found
+     399             : 
+     400             :   int    fflag, i, iFlag, j, spass, stry, tFlag, vpass, vtry;
+     401             :   double a, a1, a3, a7, b, betas, betav, c, d, e, f, g, h, oss, ots, otv, ovv, s, ss, ts, tss, tv, tvv, u, ui, v, vi, vv;
+     402             :   double qk[MDP1], svk[MDP1];
+     403             : 
+     404       13357 :   *NZ   = 0;
+     405       13357 :   betav = betas = 0.25;
+     406       13357 :   u             = -(2.0 * sr);
+     407       13357 :   oss           = sr;
+     408       13357 :   ovv = v = bnd;
+     409             : 
+     410             :   // Evaluate polynomial by synthetic division
+     411       13357 :   QuadSD_ak1(NN, u, v, p, qp, &a, &b);
+     412             : 
+     413       13357 :   tFlag = calcSC_ak1(N, a, b, &a1, &a3, &a7, &c, &d, &e, &f, &g, &h, K, u, v, qk);
+     414             : 
+     415       32062 :   for (j = 0; j < L2; j++) {
+     416             :     // Calculate next K polynomial and estimate v
+     417       32033 :     nextK_ak1(N, tFlag, a, b, a1, &a3, &a7, K, qk, qp);
+     418       32033 :     tFlag = calcSC_ak1(N, a, b, &a1, &a3, &a7, &c, &d, &e, &f, &g, &h, K, u, v, qk);
+     419       32033 :     newest_ak1(tFlag, &ui, &vi, a, a1, a3, a7, b, c, d, f, g, h, u, v, K, N, p);
+     420             : 
+     421       32033 :     vv = vi;
+     422             : 
+     423             :     // Estimate s
+     424             : 
+     425       32033 :     ss = ((K[N - 1] != 0.0) ? -(p[N] / K[N - 1]) : 0.0);
+     426             : 
+     427       32033 :     ts = tv = 1.0;
+     428             : 
+     429       32033 :     if ((j != 0) && (tFlag != 3)) {
+     430             :       // Compute relative measures of convergence of s and v sequences
+     431             : 
+     432       18676 :       tv = ((vv != 0.0) ? fabs((vv - ovv) / vv) : tv);
+     433       18676 :       ts = ((ss != 0.0) ? fabs((ss - oss) / ss) : ts);
+     434             : 
+     435             :       // If decreasing, multiply the two most recent convergence measures
+     436             : 
+     437       18676 :       tvv = ((tv < otv) ? tv * otv : 1.0);
+     438       18676 :       tss = ((ts < ots) ? ts * ots : 1.0);
+     439             : 
+     440             :       // Compare with convergence criteria
+     441             : 
+     442       18676 :       vpass = ((tvv < betav) ? 1 : 0);
+     443       18676 :       spass = ((tss < betas) ? 1 : 0);
+     444             : 
+     445       18676 :       if ((spass) || (vpass)) {
+     446             :         // At least one sequence has passed the convergence test.
+     447             :         // Store variables before iterating
+     448             : 
+     449      117587 :         for (i = 0; i < N; i++)
+     450      102560 :           svk[i] = K[i];
+     451             : 
+     452       15027 :         s = ss;
+     453             : 
+     454             :         // Choose iteration according to the fastest converging sequence
+     455             : 
+     456       15027 :         stry = vtry = 0;
+     457       15027 :         fflag       = 1;
+     458             : 
+     459         274 :         do {
+     460       15301 :           iFlag = 1;  // Begin each loop by assuming RealIT will be called
+     461             :                       // UNLESS iFlag changed below
+     462             : 
+     463       15301 :           if ((fflag && ((fflag = 0) == 0)) && ((spass) && (!vpass || (tss < tvv)))) {
+     464             :             ;  // Do nothing. Provides a quick "short circuit".
+     465             :           }    // End if (fflag)
+     466             : 
+     467             :           else {  // else !fflag
+     468        8201 :             QuadIT_ak1(N, NZ, ui, vi, szr, szi, lzr, lzi, qp, NN, &a, &b, p, qk, &a1, &a3, &a7, &d, &e, &f, &g, &h, K);
+     469             : 
+     470        8201 :             if ((*NZ) > 0)
+     471        5888 :               return;
+     472             : 
+     473             :             // Quadratic iteration has failed. Flag that it has been tried and
+     474             :             // decrease the
+     475             :             // convergence criterion
+     476             : 
+     477        2313 :             vtry = 1;
+     478        2313 :             betav *= 0.25;
+     479             : 
+     480             :             // Try linear iteration if it has not been tried and the s sequence
+     481             :             // is converging
+     482        2313 :             if (stry || (!spass)) {
+     483         904 :               iFlag = 0;
+     484             :             }  // End if (stry || (!spass))
+     485             :             else {
+     486        9856 :               for (i = 0; i < N; i++)
+     487        8447 :                 K[i] = svk[i];
+     488             :             }  // End if (stry || !spass)
+     489             : 
+     490             :           }  // End else !fflag
+     491             : 
+     492        9413 :           if (iFlag != 0) {
+     493        8509 :             RealIT_ak1(&iFlag, NZ, &s, N, p, NN, qp, szr, szi, K, qk);
+     494             : 
+     495        8509 :             if ((*NZ) > 0)
+     496        7440 :               return;
+     497             : 
+     498             :             // Linear iteration has failed. Flag that it has been tried and
+     499             :             // decrease the
+     500             :             // convergence criterion
+     501             : 
+     502        1069 :             stry = 1;
+     503        1069 :             betas *= 0.25;
+     504             : 
+     505        1069 :             if (iFlag != 0) {
+     506             :               // If linear iteration signals an almost double real zero, attempt
+     507             :               // quadratic iteration
+     508             : 
+     509          75 :               ui = -(s + s);
+     510          75 :               vi = s * s;
+     511          75 :               continue;
+     512             : 
+     513             :             }  // End if (iFlag != 0)
+     514             :           }    // End if (iFlag != 0)
+     515             : 
+     516             :           // Restore variables
+     517       15316 :           for (i = 0; i < N; i++)
+     518       13418 :             K[i] = svk[i];
+     519             : 
+     520             :           // Try quadratic iteration if it has not been tried and the v sequence
+     521             :           // is converging
+     522             : 
+     523        1973 :         } while (vpass && !vtry);  // End do-while loop
+     524             : 
+     525             :         // Re-compute qp and scalar values to continue the second stage
+     526             : 
+     527        1699 :         QuadSD_ak1(NN, u, v, p, qp, &a, &b);
+     528        1699 :         tFlag = calcSC_ak1(N, a, b, &a1, &a3, &a7, &c, &d, &e, &f, &g, &h, K, u, v, qk);
+     529             : 
+     530             :       }  // End if ((spass) || (vpass))
+     531             : 
+     532             :     }  // End if ((j != 0) && (tFlag != 3))
+     533             : 
+     534       18705 :     ovv = vv;
+     535       18705 :     oss = ss;
+     536       18705 :     otv = tv;
+     537       18705 :     ots = ts;
+     538             :   }  // End for j
+     539             : 
+     540          29 :   return;
+     541             : }  // End Fxshfr_ak1
+     542             : 
+     543      117836 : void QuadSD_ak1(int NN, double u, double v, double p[MDP1], double q[MDP1], double* a, double* b) {
+     544             :   // Divides p by the quadratic 1, u, v placing the quotient in q and the
+     545             :   // remainder in a, b
+     546             : 
+     547             :   int i;
+     548             : 
+     549      117836 :   q[0] = *b = p[0];
+     550      117836 :   q[1] = *a = -((*b) * u) + p[1];
+     551             : 
+     552      729339 :   for (i = 2; i < NN; i++) {
+     553      611503 :     q[i] = -((*a) * u + (*b) * v) + p[i];
+     554      611503 :     *b   = (*a);
+     555      611503 :     *a   = q[i];
+     556             :   }  // End for i
+     557             : 
+     558      117836 :   return;
+     559             : }  // End QuadSD_ak1
+     560             : 
+     561       80295 : int calcSC_ak1(int N, double a, double b, double* a1, double* a3, double* a7, double* c, double* d, double* e, double* f, double* g, double* h, double K[MDP1],
+     562             :                double u, double v, double qk[MDP1]) {
+     563             :   // This routine calculates scalar quantities used to compute the next K
+     564             :   // polynomial and
+     565             :   // new estimates of the quadratic coefficients.
+     566             : 
+     567             :   // calcSC - integer variable set here indicating how the calculations are
+     568             :   // normalized
+     569             :   //  to avoid overflow.
+     570             : 
+     571       80295 :   int dumFlag = 3;  // TYPE = 3 indicates the quadratic is almost a factor of K
+     572             : 
+     573             :   // Synthetic division of K by the quadratic 1, u, v
+     574       80295 :   QuadSD_ak1(N, u, v, K, qk, c, d);
+     575             : 
+     576       80295 :   if (fabs((*c)) <= (10.0 * DBL_EPSILON * fabs(K[N - 1]))) {
+     577           0 :     if (fabs((*d)) <= (10.0 * DBL_EPSILON * fabs(K[N - 2])))
+     578           0 :       return dumFlag;
+     579             :   }  // End if (fabs(c) <= (100.0*DBL_EPSILON*fabs(K[N - 1])))
+     580             : 
+     581       80295 :   *h = v * b;
+     582       80295 :   if (fabs((*d)) >= fabs((*c))) {
+     583       60953 :     dumFlag = 2;  // TYPE = 2 indicates that all formulas are divided by d
+     584       60953 :     *e      = a / (*d);
+     585       60953 :     *f      = (*c) / (*d);
+     586       60953 :     *g      = u * b;
+     587       60953 :     *a3     = (*e) * ((*g) + a) + (*h) * (b / (*d));
+     588       60953 :     *a1     = -a + (*f) * b;
+     589       60953 :     *a7     = (*h) + ((*f) + u) * a;
+     590             :   }  // End if(fabs(d) >= fabs(c))
+     591             :   else {
+     592       19342 :     dumFlag = 1;  // TYPE = 1 indicates that all formulas are divided by c;
+     593       19342 :     *e      = a / (*c);
+     594       19342 :     *f      = (*d) / (*c);
+     595       19342 :     *g      = (*e) * u;
+     596       19342 :     *a3     = (*e) * a + ((*g) + (*h) / (*c)) * b;
+     597       19342 :     *a1     = -(a * ((*d) / (*c))) + b;
+     598       19342 :     *a7     = (*g) * (*d) + (*h) * (*f) + a;
+     599             :   }  // End else
+     600             : 
+     601       80295 :   return dumFlag;
+     602             : }  // End calcSC_ak1
+     603             : 
+     604       48646 : void nextK_ak1(int N, int tFlag, double a, double b, double a1, double* a3, double* a7, double K[MDP1], double qk[MDP1], double qp[MDP1]) {
+     605             :   // Computes the next K polynomials using the scalars computed in calcSC_ak1
+     606             : 
+     607             :   int    i;
+     608             :   double temp;
+     609             : 
+     610       48646 :   if (tFlag == 3) {  // Use unscaled form of the recurrence
+     611           0 :     K[1] = K[0] = 0.0;
+     612             : 
+     613           0 :     for (i = 2; i < N; i++)
+     614           0 :       K[i] = qk[i - 2];
+     615             : 
+     616           0 :     return;
+     617             :   }  // End if (tFlag == 3)
+     618             : 
+     619       48646 :   temp = ((tFlag == 1) ? b : a);
+     620             : 
+     621       48646 :   if (fabs(a1) > (10.0 * DBL_EPSILON * fabs(temp))) {
+     622             :     // Use scaled form of the recurrence
+     623             : 
+     624       48646 :     (*a7) /= a1;
+     625       48646 :     (*a3) /= a1;
+     626       48646 :     K[0] = qp[0];
+     627       48646 :     K[1] = -((*a7) * qp[0]) + qp[1];
+     628             : 
+     629      292585 :     for (i = 2; i < N; i++)
+     630      243939 :       K[i] = -((*a7) * qp[i - 1]) + (*a3) * qk[i - 2] + qp[i];
+     631             : 
+     632             :   }  // End if (fabs(a1) > (10.0*DBL_EPSILON*fabs(temp)))
+     633             :   else {
+     634             :     // If a1 is nearly zero, then use a special form of the recurrence
+     635             : 
+     636           0 :     K[0] = 0.0;
+     637           0 :     K[1] = -(*a7) * qp[0];
+     638             : 
+     639           0 :     for (i = 2; i < N; i++)
+     640           0 :       K[i] = -((*a7) * qp[i - 1]) + (*a3) * qk[i - 2];
+     641             :   }  // End else
+     642             : 
+     643       48646 :   return;
+     644             : 
+     645             : }  // End nextK_ak1
+     646             : 
+     647       48626 : void newest_ak1(int tFlag, double* uu, double* vv, double a, double a1, double a3, double a7, double b, double c, double d, double f, double g, double h,
+     648             :                 double u, double v, double K[MDP1], int N, double p[MDP1]) {
+     649             :   // Compute new estimates of the quadratic coefficients using the scalars
+     650             :   // computed in calcSC_ak1
+     651             : 
+     652             :   double a4, a5, b1, b2, c1, c2, c3, c4, temp;
+     653             : 
+     654       48626 :   (*vv) = (*uu) = 0.0;  // The quadratic is zeroed
+     655             : 
+     656       48626 :   if (tFlag != 3) {
+     657       48626 :     if (tFlag != 2) {
+     658       10993 :       a4 = a + u * b + h * f;
+     659       10993 :       a5 = c + (u + v * f) * d;
+     660             :     }       // End if (tFlag != 2)
+     661             :     else {  // else tFlag == 2
+     662       37633 :       a4 = (a + g) * f + h;
+     663       37633 :       a5 = (f + u) * c + v * d;
+     664             :     }  // End else tFlag == 2
+     665             : 
+     666             :     // Evaluate new quadratic coefficients
+     667             : 
+     668       48626 :     b1   = -K[N - 1] / p[N];
+     669       48626 :     b2   = -(K[N - 2] + b1 * p[N - 1]) / p[N];
+     670       48626 :     c1   = v * b2 * a1;
+     671       48626 :     c2   = b1 * a7;
+     672       48626 :     c3   = b1 * b1 * a3;
+     673       48626 :     c4   = -(c2 + c3) + c1;
+     674       48626 :     temp = -c4 + a5 + b1 * a4;
+     675       48626 :     if (temp != 0.0) {
+     676       48603 :       *uu = -((u * (c3 + c2) + v * (b1 * a1 + b2 * a7)) / temp) + u;
+     677       48603 :       *vv = v * (1.0 + c4 / temp);
+     678             :     }  // End if (temp != 0)
+     679             : 
+     680             :   }  // End if (tFlag != 3)
+     681             : 
+     682       48626 :   return;
+     683             : }  // End newest_ak1
+     684             : 
+     685        8201 : void QuadIT_ak1(int N, int* NZ, double uu, double vv, double* szr, double* szi, double* lzr, double* lzi, double qp[MDP1], int NN, double* a, double* b,
+     686             :                 double p[MDP1], double qk[MDP1], double* a1, double* a3, double* a7, double* d, double* e, double* f, double* g, double* h, double K[MDP1]) {
+     687             :   // Variable-shift K-polynomial iteration for a quadratic factor converges only
+     688             :   // if the
+     689             :   // zeros are equimodular or nearly so.
+     690             : 
+     691        8201 :   int    i, j = 0, tFlag, triedFlag = 0;
+     692             :   double c, ee, mp, omp, relstp, t, u, ui, v, vi, zm;
+     693             : 
+     694        8201 :   *NZ = 0;   // Number of zeros found
+     695        8201 :   u   = uu;  // uu and vv are coefficients of the starting quadratic
+     696        8201 :   v   = vv;
+     697             : 
+     698       16586 :   do {
+     699       24787 :     Quad_ak1(1.0, u, v, szr, szi, lzr, lzi);
+     700             : 
+     701             :     // Return if roots of the quadratic are real and not close to multiple or
+     702             :     // nearly
+     703             :     // equal and of opposite sign.
+     704             : 
+     705       24787 :     if (fabs(fabs(*szr) - fabs(*lzr)) > 0.01 * fabs(*lzr))
+     706        2306 :       break;
+     707             : 
+     708             :     // Evaluate polynomial by quadratic synthetic division
+     709             : 
+     710       22481 :     QuadSD_ak1(NN, u, v, p, qp, a, b);
+     711             : 
+     712       22481 :     mp = fabs(-((*szr) * (*b)) + (*a)) + fabs((*szi) * (*b));
+     713             : 
+     714             :     // Compute a rigorous bound on the rounding error in evaluating p
+     715             : 
+     716       22481 :     zm = sqrt(fabs(v));
+     717       22481 :     ee = 2.0 * fabs(qp[0]);
+     718       22481 :     t  = -((*szr) * (*b));
+     719             : 
+     720      150973 :     for (i = 1; i < N; i++)
+     721      128492 :       ee = ee * zm + fabs(qp[i]);
+     722             : 
+     723       22481 :     ee = ee * zm + fabs((*a) + t);
+     724       22481 :     ee = (9.0 * ee + 2.0 * fabs(t) - 7.0 * (fabs((*a) + t) + zm * fabs((*b)))) * DBL_EPSILON;
+     725             : 
+     726             :     // Iteration has converged sufficiently if the polynomial value is less than
+     727             :     // 20 times this bound
+     728             : 
+     729       22481 :     if (mp <= 20.0 * ee) {
+     730        5888 :       *NZ = 2;
+     731        5888 :       break;
+     732             :     }  // End if (mp <= 20.0*ee)
+     733             : 
+     734       16593 :     j++;
+     735             : 
+     736             :     // Stop iteration after 20 steps
+     737       16593 :     if (j > 20)
+     738           0 :       break;
+     739             : 
+     740       16593 :     if (j >= 2) {
+     741       10363 :       if ((relstp <= 0.01) && (mp >= omp) && (!triedFlag)) {
+     742             :         // A cluster appears to be stalling the convergence. Five fixed shift
+     743             :         // steps are taken with a u, v close to the cluster.
+     744             : 
+     745           4 :         relstp = ((relstp < DBL_EPSILON) ? sqrt(DBL_EPSILON) : sqrt(relstp));
+     746             : 
+     747           4 :         u -= u * relstp;
+     748           4 :         v += v * relstp;
+     749             : 
+     750           4 :         QuadSD_ak1(NN, u, v, p, qp, a, b);
+     751             : 
+     752          24 :         for (i = 0; i < 5; i++) {
+     753          20 :           tFlag = calcSC_ak1(N, *a, *b, a1, a3, a7, &c, d, e, f, g, h, K, u, v, qk);
+     754          20 :           nextK_ak1(N, tFlag, *a, *b, *a1, a3, a7, K, qk, qp);
+     755             :         }  // End for i
+     756             : 
+     757           4 :         triedFlag = 1;
+     758           4 :         j         = 0;
+     759             : 
+     760             :       }  // End if ((relstp <= 0.01) && (mp >= omp) && (!triedFlag))
+     761             : 
+     762             :     }  // End if (j >= 2)
+     763             : 
+     764       16593 :     omp = mp;
+     765             : 
+     766             :     // Calculate next K polynomial and new u and v
+     767             : 
+     768       16593 :     tFlag = calcSC_ak1(N, *a, *b, a1, a3, a7, &c, d, e, f, g, h, K, u, v, qk);
+     769       16593 :     nextK_ak1(N, tFlag, *a, *b, *a1, a3, a7, K, qk, qp);
+     770       16593 :     tFlag = calcSC_ak1(N, *a, *b, a1, a3, a7, &c, d, e, f, g, h, K, u, v, qk);
+     771       16593 :     newest_ak1(tFlag, &ui, &vi, *a, *a1, *a3, *a7, *b, c, *d, *f, *g, *h, u, v, K, N, p);
+     772             : 
+     773             :     // If vi is zero, the iteration is not converging
+     774       16593 :     if (vi != 0) {
+     775       16586 :       relstp = fabs((-v + vi) / vi);
+     776       16586 :       u      = ui;
+     777       16586 :       v      = vi;
+     778             :     }                 // End if (vi != 0)
+     779       16593 :   } while (vi != 0);  // End do-while loop
+     780             : 
+     781       16402 :   return;
+     782             : 
+     783             : }  // End QuadIT_ak1
+     784             : 
+     785        8509 : void RealIT_ak1(int* iFlag, int* NZ, double* sss, int N, double p[MDP1], int NN, double qp[MDP1], double* szr, double* szi, double K[MDP1], double qk[MDP1]) {
+     786             :   // Variable-shift H-polynomial iteration for a real zero
+     787             : 
+     788             :   // sss  - starting iterate
+     789             :   // NZ - number of zeros found
+     790             :   // iFlag  - flag to indicate a pair of zeros near real axis
+     791             : 
+     792        8509 :   int    i, j = 0, nm1 = N - 1;
+     793             :   double ee, kv, mp, ms, omp, pv, s, t;
+     794             : 
+     795        8509 :   *iFlag = *NZ = 0;
+     796        8509 :   s            = *sss;
+     797             : 
+     798             :   for (;;) {
+     799       38170 :     qp[0] = pv = p[0];
+     800             : 
+     801             :     // Evaluate p at s
+     802      292210 :     for (i = 1; i < NN; i++)
+     803      254040 :       qp[i] = pv = pv * s + p[i];
+     804             : 
+     805       38170 :     mp = fabs(pv);
+     806             : 
+     807             :     // Compute a rigorous bound on the error in evaluating p
+     808             : 
+     809       38170 :     ms = fabs(s);
+     810       38170 :     ee = 0.5 * fabs(qp[0]);
+     811      292210 :     for (i = 1; i < NN; i++)
+     812      254040 :       ee = ee * ms + fabs(qp[i]);
+     813             : 
+     814             :     // Iteration has converged sufficiently if the polynomial value is less than
+     815             :     // 20 times this bound
+     816             : 
+     817       38170 :     if (mp <= 20.0 * DBL_EPSILON * (2.0 * ee - mp)) {
+     818        7440 :       *NZ  = 1;
+     819        7440 :       *szr = s;
+     820        7440 :       *szi = 0.0;
+     821        7440 :       break;
+     822             :     }  // End if (mp <= 20.0*DBL_EPSILON*(2.0*ee - mp))
+     823             : 
+     824       30730 :     j++;
+     825             : 
+     826             :     // Stop iteration after 10 steps
+     827             : 
+     828       30730 :     if (j > 10)
+     829         994 :       break;
+     830             : 
+     831       29736 :     if (j >= 2) {
+     832       21333 :       if ((fabs(t) <= 0.001 * fabs(-t + s)) && (mp > omp)) {
+     833             :         // A cluster of zeros near the real axis has been encountered;
+     834             :         // Return with iFlag set to initiate a quadratic iteration
+     835             : 
+     836          75 :         *iFlag = 1;
+     837          75 :         *sss   = s;
+     838          75 :         break;
+     839             :       }  // End if ((fabs(t) <= 0.001*fabs(s - t)) && (mp > omp))
+     840             : 
+     841             :     }  // End if (j >= 2)
+     842             : 
+     843             :     // Return if the polynomial value has increased significantly
+     844             : 
+     845       29661 :     omp = mp;
+     846             : 
+     847             :     // Compute t, the next polynomial and the new iterate
+     848       29661 :     qk[0] = kv = K[0];
+     849      195884 :     for (i = 1; i < N; i++)
+     850      166223 :       qk[i] = kv = kv * s + K[i];
+     851             : 
+     852       29661 :     if (fabs(kv) > fabs(K[nm1]) * 10.0 * DBL_EPSILON) {
+     853             :       // Use the scaled form of the recurrence if the value of K at s is
+     854             :       // non-zero
+     855       29661 :       t    = -(pv / kv);
+     856       29661 :       K[0] = qp[0];
+     857      195884 :       for (i = 1; i < N; i++)
+     858      166223 :         K[i] = t * qk[i - 1] + qp[i];
+     859             :     }       // End if (fabs(kv) > fabs(K[nm1])*10.0*DBL_EPSILON)
+     860             :     else {  // else (fabs(kv) <= fabs(K[nm1])*10.0*DBL_EPSILON)
+     861             :       // Use unscaled form
+     862           0 :       K[0] = 0.0;
+     863           0 :       for (i = 1; i < N; i++)
+     864           0 :         K[i] = qk[i - 1];
+     865             :     }  // End else (fabs(kv) <= fabs(K[nm1])*10.0*DBL_EPSILON)
+     866             : 
+     867       29661 :     kv = K[0];
+     868      195884 :     for (i = 1; i < N; i++)
+     869      166223 :       kv = kv * s + K[i];
+     870             : 
+     871       29661 :     t = ((fabs(kv) > (fabs(K[nm1]) * 10.0 * DBL_EPSILON)) ? -(pv / kv) : 0.0);
+     872             : 
+     873       29661 :     s += t;
+     874             : 
+     875             :   }  // End infinite for loop
+     876             : 
+     877        8509 :   return;
+     878             : 
+     879             : }  // End RealIT_ak1
+     880             : 
+     881       26711 : void Quad_ak1(double a, double b1, double c, double* sr, double* si, double* lr, double* li) {
+     882             :   // Calculates the zeros of the quadratic a*Z^2 + b1*Z + c
+     883             :   // The quadratic formula, modified to avoid overflow, is used to find the
+     884             :   // larger zero if the
+     885             :   // zeros are real and both zeros are complex. The smaller real zero is found
+     886             :   // directly from
+     887             :   // the product of the zeros c/a.
+     888             : 
+     889             :   double b, d, e;
+     890             : 
+     891       26711 :   *sr = *si = *lr = *li = 0.0;
+     892             : 
+     893       26711 :   if (a == 0) {
+     894           0 :     *sr = ((b1 != 0) ? -(c / b1) : *sr);
+     895           0 :     return;
+     896             :   }  // End if (a == 0))
+     897             : 
+     898       26711 :   if (c == 0) {
+     899           0 :     *lr = -(b1 / a);
+     900           0 :     return;
+     901             :   }  // End if (c == 0)
+     902             : 
+     903             :   // Compute discriminant avoiding overflow
+     904             : 
+     905       26711 :   b = b1 / 2.0;
+     906       26711 :   if (fabs(b) < fabs(c)) {
+     907       11689 :     e = ((c >= 0) ? a : -a);
+     908       11689 :     e = -e + b * (b / fabs(c));
+     909       11689 :     d = sqrt(fabs(e)) * sqrt(fabs(c));
+     910             :   }       // End if (fabs(b) < fabs(c))
+     911             :   else {  // Else (fabs(b) >= fabs(c))
+     912       15022 :     e = -((a / b) * (c / b)) + 1.0;
+     913       15022 :     d = sqrt(fabs(e)) * (fabs(b));
+     914             :   }  // End else (fabs(b) >= fabs(c))
+     915             : 
+     916       26711 :   if (e >= 0) {
+     917             :     // Real zeros
+     918             : 
+     919        3904 :     d   = ((b >= 0) ? -d : d);
+     920        3904 :     *lr = (-b + d) / a;
+     921        3904 :     *sr = ((*lr != 0) ? (c / (*lr)) / a : *sr);
+     922             :   }       // End if (e >= 0)
+     923             :   else {  // Else (e < 0)
+     924             :     // Complex conjugate zeros
+     925             : 
+     926       22807 :     *lr = *sr = -(b / a);
+     927       22807 :     *si       = fabs(d / a);
+     928       22807 :     *li       = -(*si);
+     929             :   }  // End else (e < 0)
+     930             : 
+     931       26711 :   return;
+     932             : }  // End Quad_ak1
+     933             : 
+     934             : 
+     935             : }  // namespace rpoly_impl
+     936             : 
+     937             : //}
+     938             : 
+     939        2862 : void rpolyWrapper(double* coefficients_decreasing, int* degree, double* roots_real, double* roots_imag) {
+     940        2862 :   rpoly_impl::rpoly_ak1(coefficients_decreasing, degree, roots_real, roots_imag);
+     941        2862 : }
+     942             : 
+     943             : }  // namespace eth_trajectory_generation
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.overview.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.overview.html new file mode 100644 index 0000000000..b19041617f --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.overview.html @@ -0,0 +1,256 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.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 + + +
    + Top

    + Overview +
    + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.png b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/rpoly/rpoly_ak1.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..4fb161a43fcecef41f14c4a3ab9e37e48c99fc0a GIT binary patch literal 4844 zcmVQV1ucv96062NA0M6yJsq|Hz5_+Gdj{_(LI-CL^GBnMS2iBK5c9Jv3o?#CxmxlL;gne#4xtJ-w=?iJvqf+=;ms|>wuo6d534Zpp9 zay5t7(ECR+S6O#ZppChLr zI}R|#B+iIBA(wl06}!*sRZD)nZN^Tmq7g%Vphpzm_Pfxh^&o-pLGjw?Z1jhW1 zOyahPb^?T?@Z>;TQv89{{4cNo)U0*@|H7$&yMAAf=+bkWO~7QnX=soJi(*ADJ_|)c z6i~xJ8e@bZPeBqp*ItOE>QMqs>J;$viysAWwhs~_YBIDe1vb@>I-HiE{ z%j;qRY?&U&+`6`Q<3n4#m^j@#sHT%LdAu zzERM_fa{u12G0RV4ouz^FcV;YGfzYzp~p-;7P~8NZy#^~D_t0Cu*12BwpJ|Kbz=k( z)6jpi*kJVLM4IlGHD@u%8%py4dOic%^~W{pp@tt%9`f<%cmgAgJ9F%pq2Rg551w;q2Hls zF7-?SjBPFi7CBUq`QJHMK!DldMywj@F%zQRgAbR0ov3T3d2WS7t)Ywfd5IaKc*laY zLvQ%SL_~FM8x2i7-u0&@?y!#=2Fx%OdVC(=GtA;v|IaFjK1rScIi8TTfKL1|*h-IP z8#bAZD8oSvuSa5B2h=b{UjSjxY(&GY+3`@1#1zhc9c2nmKqi>u*AkEnP5$dF1~sgB zwK-vu2Zq+<=fY+H^0k!}ON<*_OvBrG10?R3>Wi;jk3`hg$ja9?jSdU$;v9$7&0_zl462!2RW zL#nC~lIVQU(UvN-ZGd<~9064j^LLCE!=jKd#*cvWh3ClhBh>1b>Uo@fhjq=`)-^84l3eMK2Z z3+iHGv1Xtr1V~cR%|L=Oksbs4mE|Ml^n3jl5*DGHceXe8OWS^saf1kt^*Et^4oZCH z+BPZo_=G8TXuRAGr+_k-j>J14D07vBt5!|UX1CKCFs8ZsRvZ@NF2}4oz_@vjZ3T7N zdw@eEjD$2k(F#&-S6RP(|Er6Y32?C_ZYhzFg`1||wbV@G(_$;B^$FZpE8B99zqZMW*z@dhqi?0GC zpO$oYmx%GKCC7z6Ik)TZYFcB%w#7X&ZeBejK;4v$KwI04=jcm&!~on43qUbn;R%Wj z%Nw(bN%cA%qF~~x>M^=|lokUrwRmG4lel}6A~hZZ4Rh4&B%&jdx*Sz3tBO4ir}h@Z zQCFfh=ZQGdz%kwxh?Kf|gmWa}x0phGI>)eB!~F~0SmsFc+E~H0w?g4>+AmFuTme1G z05B#10!{F!A+R}E)ZRsp2^nwX_GP4h@jIk65mxj8S9q96zi|GU$JJHKp#{Gxf3||z znqUj6!T}iMDpNs_q_X2vjx0+$*nvc1Q;uuY)d^pZ0+yk>ENSQ1wTVB7n>k*oGDRR? zxI&cn7Cs1w07Nehhz@)66F|-Qv?ChCWiUR{9hFzixFTKW#q0*e$0bh1@A-@yP;wQm zgPx{?@R9Z>7JyXzs8mUxswL)!MnU=sJymcWDwWGmVX;jxc2Nf1HLs82h~J+5ZveYM z0E8Ld_Z0?O!uibkCMuSKE{Xni0M-8Q!xhM6w-Y)?(vmlm(&f>mjg-Xr2c-_$2|G%C@P} zt9Q(CEmd;?4rjkL;GFTFbH7$;r2`@b&+Qs1Xf@K~Zu^q@3i%y! zrP^-j2{72BR7u>ht(bdOj_M%w&wlnfSUkAuz+M>ZkaI?s>%2(*;MzqQ*C(`VTRH^qopR`2Z}R&tvk!mfsm$n!(Mi1EP~`F3x|3HdQ=VC zn|nTCrlVW%w>w87Q~)rvn9^gQw7`VoR}{q}9Ir{G)6g1?v;2h~htQ*?R~$>uhUqtw z4oGQi*~r`t6EqW`ZnaYCF8@A~@?p$~ry|BedBD$TjFfTRb-p_Vl&hg8mWJgQFqBmHuv=aaLP9*?X3rmJ1o9fPgnE*&8e1VWA zN59O$#{V+`z&*NLA(WA`ajNb%9I`y#squ3odbk+S&Cj4p%U|{CM)yY9EX@+`=_WN) zvEP;bQ0vaY<8VsjKJ*|liHyu_HsF$sRT@w;@pg=_hu!vOq2{=GEoSxU1jcx*k>L?s(Lmhi)3Ocsa+N4f=Ce>DUJ#bdc{DWy1!syx&Upa|dTd-1 z%c8X<+->*ykrAhy`QwR#VAKUySGY|-xU9*&{!I&ppOTON{eA0lgfgL1{`IPR5#I@g zN)@S*!{R;2*JB<#KS%0F3!odHH9dNZDM`(YYF-t)?}r8m(2J5@^wEdidLqW4viB!}xhSuGTbo-$)zy`J_4w%mJ3nM#RnxgpO5?aHH|ZFe_|7cHrHV? z6??1?7;EE_Lc|4Ihv({3DU@`F4!5v#7G{bhruQJFn$5&1I3BH!M$J_fmFHlFdUTuk z3fF$uj6amIO~CA~vPSq}_U9YjXbL&xcIk^NrAI|`?1risBo_jq^%D?${5toplR5zP z5W?Omw4cHi&?{=R9RH^Vbmk~$0lR6J5%!rSzXgu{f_EkahWHYM?y4z*gbNc|1yBs= zAB|@~Kk%x6PT`bo6XnB{`nyUI-lTvEA!sWZ(% z5=w_{)z0YALT*J?&d|)2Q{fJ+83?U^5Q>B3iN78+;rQi8tbfH6s6OopKv*deJjTC( zN>3MeJ~FhJ$PLG%yD%9lG{5nSZX@-Jw0~kpNe6-jYwS|sQ=L7d9tUZwWYfArR*3%B zI(t%iSpvRdsL=JiMu~mKk5WA^Wh{_dyOCdG6>P=fa=3B~Fg$cb(+u_)zX;7#0@)vP z01JR@V2RlW1FY^bbP?JptzEG~LzjmDV5A+RpQ@Vp)fZQZa)-CwS1?e66katH48C)k zoRrT?ZbCfv;-V?UV=DsZBR#UmW=|n(_CsUnNqV`mRl#qo4&3M>x)8#weTeS>%;gT0 zK#x&C;T&M(GTt=t4V8yvl0@&9Ldvr+hkHz)#_*d>VgOQ|%uc_J_wJkd>O|3g^q2FE z{sUtlt~?yG{&z;xcf+vM7LVknB1AZlK47rNYbTehyFb!Om}4ZJ8g+69 z5Pq*q`d{J~vgM7C^!uuF@Vi~y+8zrSA>oIfrg10g68{nO{8OIh5DBB_hNE~4E@s#z z)dI-OgxZ=eX%>a8Pa>7d$~ok2L!e)rHd`UoOAy_}W5K0Y(4GT3HGj z`lMRJ0D|1{rlfcJmZXu-e>T3 z1bmq^0qHJ#nT?`}e8BHeM0Os$L3JzFLmnTGQ-mtS6j8tmk5~Q^xBmdrG;uIz Sui)JP0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - segment.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:5413540.0 %
    Date:2024-04-09 22:03:33Functions:61346.2 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::printSegment(std::basic_ostream<char, std::char_traits<char> >&, eth_trajectory_generation::Segment const&, int)0
    eth_trajectory_generation::Segment::offsetSegment(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)0
    eth_trajectory_generation::operator<<(std::basic_ostream<char, std::char_traits<char> >&, eth_trajectory_generation::Segment const&)0
    eth_trajectory_generation::operator<<(std::basic_ostream<char, std::char_traits<char> >&, std::vector<eth_trajectory_generation::Segment, std::allocator<eth_trajectory_generation::Segment> > const&)0
    eth_trajectory_generation::Segment::getSegmentWithSingleDimension(int, eth_trajectory_generation::Segment*) const0
    eth_trajectory_generation::Segment::getSegmentWithAppendedDimension(eth_trajectory_generation::Segment const&, eth_trajectory_generation::Segment*) const0
    eth_trajectory_generation::Segment::operator==(eth_trajectory_generation::Segment const&) const0
    eth_trajectory_generation::Segment::computeMinMaxMagnitudeCandidates(int, double, double, std::vector<int, std::allocator<int> > const&, std::vector<eth_trajectory_generation::Extremum, std::allocator<eth_trajectory_generation::Extremum> >*) const2970
    eth_trajectory_generation::Segment::selectMinMaxMagnitudeFromCandidates(int, double, double, std::vector<int, std::allocator<int> > const&, std::vector<eth_trajectory_generation::Extremum, std::allocator<eth_trajectory_generation::Extremum> > const&, eth_trajectory_generation::Extremum*, eth_trajectory_generation::Extremum*) const2970
    eth_trajectory_generation::Segment::computeMinMaxMagnitudeCandidateTimes(int, double, double, std::vector<int, std::allocator<int> > const&, std::vector<double, std::allocator<double> >*) const2970
    eth_trajectory_generation::Segment::evaluate(double, int) const7040
    eth_trajectory_generation::Segment::operator[](unsigned long) const68304
    eth_trajectory_generation::Segment::operator[](unsigned long)75604
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.func.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.func.html new file mode 100644 index 0000000000..27d045ba1b --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.func.html @@ -0,0 +1,132 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - segment.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:5413540.0 %
    Date:2024-04-09 22:03:33Functions:61346.2 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::printSegment(std::basic_ostream<char, std::char_traits<char> >&, eth_trajectory_generation::Segment const&, int)0
    eth_trajectory_generation::Segment::offsetSegment(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)0
    eth_trajectory_generation::Segment::operator[](unsigned long)75604
    eth_trajectory_generation::operator<<(std::basic_ostream<char, std::char_traits<char> >&, eth_trajectory_generation::Segment const&)0
    eth_trajectory_generation::operator<<(std::basic_ostream<char, std::char_traits<char> >&, std::vector<eth_trajectory_generation::Segment, std::allocator<eth_trajectory_generation::Segment> > const&)0
    eth_trajectory_generation::Segment::getSegmentWithSingleDimension(int, eth_trajectory_generation::Segment*) const0
    eth_trajectory_generation::Segment::getSegmentWithAppendedDimension(eth_trajectory_generation::Segment const&, eth_trajectory_generation::Segment*) const0
    eth_trajectory_generation::Segment::computeMinMaxMagnitudeCandidates(int, double, double, std::vector<int, std::allocator<int> > const&, std::vector<eth_trajectory_generation::Extremum, std::allocator<eth_trajectory_generation::Extremum> >*) const2970
    eth_trajectory_generation::Segment::selectMinMaxMagnitudeFromCandidates(int, double, double, std::vector<int, std::allocator<int> > const&, std::vector<eth_trajectory_generation::Extremum, std::allocator<eth_trajectory_generation::Extremum> > const&, eth_trajectory_generation::Extremum*, eth_trajectory_generation::Extremum*) const2970
    eth_trajectory_generation::Segment::computeMinMaxMagnitudeCandidateTimes(int, double, double, std::vector<int, std::allocator<int> > const&, std::vector<double, std::allocator<double> >*) const2970
    eth_trajectory_generation::Segment::evaluate(double, int) const7040
    eth_trajectory_generation::Segment::operator==(eth_trajectory_generation::Segment const&) const0
    eth_trajectory_generation::Segment::operator[](unsigned long) const68304
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.frameset.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.frameset.html new file mode 100644 index 0000000000..e356221b47 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.html new file mode 100644 index 0000000000..e1b0e14b30 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.html @@ -0,0 +1,399 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - segment.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:5413540.0 %
    Date:2024-04-09 22:03:33Functions:61346.2 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #include <eth_trajectory_generation/segment.h>
+      22             : 
+      23             : #include <cmath>
+      24             : #include <limits>
+      25             : 
+      26             : namespace eth_trajectory_generation
+      27             : {
+      28             : 
+      29             : /* operator==(const Segment& rhs)() //{ */
+      30             : 
+      31           0 : bool Segment::operator==(const Segment& rhs) const {
+      32           0 :   if (D_ != rhs.D_ || time_ != rhs.time_) {
+      33           0 :     return false;
+      34             :   } else {
+      35           0 :     for (int i = 0; i < D(); i++) {
+      36           0 :       if (polynomials_[i] != rhs[i]) {
+      37           0 :         return false;
+      38             :       }
+      39             :     }
+      40             :   }
+      41           0 :   return true;
+      42             : }
+      43             : 
+      44             : //}
+      45             : 
+      46             : /* Segment::operator[](size_t idx) //{ */
+      47             : 
+      48       75604 : Polynomial& Segment::operator[](size_t idx) {
+      49       75604 :   CHECK_LT(idx, static_cast<size_t>(D_));
+      50       75604 :   return polynomials_[idx];
+      51             : }
+      52             : 
+      53             : //}
+      54             : 
+      55             : /* Segment::operator[](size_t idx) //{ */
+      56             : 
+      57       68304 : const Polynomial& Segment::operator[](size_t idx) const {
+      58       68304 :   CHECK_LT(idx, static_cast<size_t>(D_));
+      59       68304 :   return polynomials_[idx];
+      60             : }
+      61             : 
+      62             : //}
+      63             : 
+      64             : /* evaluate() //{ */
+      65             : 
+      66        7040 : Eigen::VectorXd Segment::evaluate(double t, int derivative) const {
+      67        7040 :   Eigen::VectorXd result(D_);
+      68        7040 :   result.setZero();
+      69       35200 :   for (int d = 0; d < D_; ++d) {
+      70       28160 :     result[d] = polynomials_[d].evaluate(t, derivative);
+      71             :   }
+      72        7040 :   return result;
+      73             : }
+      74             : 
+      75             : //}
+      76             : 
+      77             : /* printSegment() //{ */
+      78             : 
+      79           0 : void printSegment(std::ostream& stream, const Segment& s, int derivative) {
+      80           0 :   CHECK(derivative >= 0 && derivative < s.N());
+      81           0 :   stream << "t: " << s.getTime() << std::endl;
+      82           0 :   stream << " coefficients for " << positionDerivativeToString(derivative) << ": " << std::endl;
+      83           0 :   for (int i = 0; i < s.D(); ++i) {
+      84           0 :     stream << "dim " << i << ": " << std::endl;
+      85           0 :     stream << s[i].getCoefficients(derivative) << std::endl;
+      86             :   }
+      87           0 : }
+      88             : 
+      89             : //}
+      90             : 
+      91             : /* operator<<(std::ostream& stream, const Segment& s) //{ */
+      92             : 
+      93           0 : std::ostream& operator<<(std::ostream& stream, const Segment& s) {
+      94           0 :   printSegment(stream, s, derivative_order::POSITION);
+      95           0 :   return stream;
+      96             : }
+      97             : 
+      98             : //}
+      99             : 
+     100             : /* operator<<(std::ostream& stream, const std::vector<Segment>& segments) //{ */
+     101             : 
+     102           0 : std::ostream& operator<<(std::ostream& stream, const std::vector<Segment>& segments) {
+     103           0 :   for (const Segment& s : segments)
+     104           0 :     stream << s << std::endl;
+     105             : 
+     106           0 :   return stream;
+     107             : }
+     108             : 
+     109             : //}
+     110             : 
+     111             : /* computeMinMaxMagnitudeCandidateTimes() //{ */
+     112             : 
+     113        2970 : bool Segment::computeMinMaxMagnitudeCandidateTimes(
+     114             : 
+     115             :     int derivative, double t_start, double t_end, const std::vector<int>& dimensions, std::vector<double>* candidate_times) const {
+     116        2970 :   CHECK_NOTNULL(candidate_times);
+     117        2970 :   candidate_times->clear();
+     118             :   // Compute magnitude derivative roots.
+     119        2970 :   if (dimensions.empty()) {
+     120           0 :     LOG(WARNING) << "No dimensions specified." << std::endl;
+     121           0 :     return false;
+     122        2970 :   } else if (dimensions.size() > 1) {
+     123         990 :     const int       n_d                           = N_ - derivative;
+     124         990 :     const int       n_dd                          = n_d - 1;
+     125         990 :     const int       convolved_coefficients_length = Polynomial::getConvolutionLength(n_d, n_dd);
+     126         990 :     Eigen::VectorXd convolved_coefficients(convolved_coefficients_length);
+     127         990 :     convolved_coefficients.setZero();
+     128        2970 :     for (int dim : dimensions) {
+     129        1980 :       if (dim < 0 || dim >= D_) {
+     130           0 :         LOG(WARNING) << "Specified dimensions " << dim << " are out of bounds [0.." << D_ - 1 << "]." << std::endl;
+     131           0 :         return false;
+     132             :       }
+     133             :       // Our coefficients are INCREASING, so when you take the derivative,
+     134             :       // only the lower powers of t have non-zero coefficients.
+     135             :       // So we take the head.
+     136        3960 :       Eigen::VectorXd d  = polynomials_[dim].getCoefficients(derivative).head(n_d);
+     137        1980 :       Eigen::VectorXd dd = polynomials_[dim].getCoefficients(derivative + 1).head(n_dd);
+     138        1980 :       convolved_coefficients += Polynomial::convolve(d, dd);
+     139             :     }
+     140         990 :     Polynomial polynomial_convolved(convolved_coefficients);
+     141             : 
+     142             :     // derivative = -1 because the convolved polynomial is the derivative
+     143             :     // already. We wish to find the minimum and maximum candidates for the
+     144             :     // integral.
+     145         990 :     if (!polynomial_convolved.computeMinMaxCandidates(t_start, t_end, -1, candidate_times)) {
+     146           0 :       return false;
+     147             :     }
+     148             :   } else {
+     149             :     // For dimension.size() == 1  we can simply evaluate the roots of the
+     150             :     // derivative.
+     151        1980 :     if (!polynomials_[dimensions[0]].computeMinMaxCandidates(t_start, t_end, derivative, candidate_times)) {
+     152           0 :       return false;
+     153             :     }
+     154             :   }
+     155        2970 :   return true;
+     156             : }
+     157             : 
+     158             : //}
+     159             : 
+     160             : /* computeMinMaxMagnitudeCandidates() //{ */
+     161             : 
+     162        2970 : bool Segment::computeMinMaxMagnitudeCandidates(
+     163             : 
+     164             :     int derivative, double t_start, double t_end, const std::vector<int>& dimensions, std::vector<Extremum>* candidates) const {
+     165        2970 :   CHECK_NOTNULL(candidates);
+     166             :   // Find candidate times (roots + start + end).
+     167        2970 :   std::vector<double> candidate_times;
+     168        2970 :   computeMinMaxMagnitudeCandidateTimes(derivative, t_start, t_end, dimensions, &candidate_times);
+     169             : 
+     170             :   // Evaluate candidate times.
+     171        2970 :   candidates->resize(candidate_times.size());
+     172       16336 :   for (size_t i = 0; i < candidate_times.size(); i++) {
+     173       13366 :     double magnitude = 0.0;
+     174       31698 :     for (int dim : dimensions) {
+     175       18332 :       magnitude += std::pow(polynomials_[dim].evaluate(candidate_times[i], derivative), 2);
+     176             :     }
+     177       13366 :     magnitude        = std::sqrt(magnitude);
+     178       13366 :     (*candidates)[i] = Extremum(candidate_times[i], magnitude, 0);
+     179             :   }
+     180             : 
+     181        5940 :   return true;
+     182             : }
+     183             : 
+     184             : //}
+     185             : 
+     186             : /* selectMinMaxMagnitudeFromCandidates() //{ */
+     187             : 
+     188        2970 : bool Segment::selectMinMaxMagnitudeFromCandidates(
+     189             : 
+     190             :     int derivative, double t_start, double t_end, const std::vector<int>& dimensions, const std::vector<Extremum>& candidates, Extremum* minimum,
+     191             :     Extremum* maximum) const {
+     192        2970 :   CHECK_NOTNULL(minimum);
+     193        2970 :   CHECK_NOTNULL(maximum);
+     194        2970 :   if (t_start > t_end) {
+     195           0 :     LOG(WARNING) << "t_start is greater than t_end.";
+     196           0 :     return false;
+     197             :   }
+     198             : 
+     199        2970 :   minimum->value = std::numeric_limits<double>::max();
+     200        2970 :   maximum->value = std::numeric_limits<double>::lowest();
+     201             : 
+     202             :   // Evaluate passed candidates.
+     203       16336 :   for (const Extremum& candidate : candidates) {
+     204       13366 :     if (candidate.time < t_start || candidate.time > t_end) {
+     205           0 :       continue;
+     206             :     }
+     207       13366 :     *maximum = std::max(*maximum, candidate);
+     208       13366 :     *minimum = std::min(*minimum, candidate);
+     209             :   }
+     210             : 
+     211        2970 :   return true;
+     212             : }
+     213             : 
+     214             : //}
+     215             : 
+     216             : /* selectMinMaxMagnitudeFromCandidates() //{ */
+     217             : 
+     218           0 : bool Segment::getSegmentWithSingleDimension(int dimension, Segment* new_segment) const {
+     219           0 :   if (dimension < 0 || dimension >= D_) {
+     220           0 :     LOG(WARNING) << "You shan't ask for a dimension that does not exist in the segment.";
+     221           0 :     return false;
+     222             :   }
+     223             : 
+     224           0 :   *new_segment      = Segment(N_, 1);
+     225           0 :   (*new_segment)[0] = polynomials_[dimension];
+     226           0 :   new_segment->setTime(time_);
+     227           0 :   return true;
+     228             : }
+     229             : 
+     230             : //}
+     231             : 
+     232             : /* getSegmentWithAppendedDimension() //{ */
+     233             : 
+     234           0 : bool Segment::getSegmentWithAppendedDimension(const Segment& segment_to_append, Segment* new_segment) const {
+     235           0 :   if (N_ == 0 || D_ == 0) {
+     236           0 :     *new_segment = segment_to_append;
+     237           0 :     return true;
+     238             :   }
+     239           0 :   if (segment_to_append.N() == 0 || segment_to_append.D() == 0) {
+     240           0 :     *new_segment = *this;
+     241           0 :     return true;
+     242             :   }
+     243             : 
+     244             :   // Get common polynomial order.
+     245           0 :   const int new_N = std::max(segment_to_append.N(), N_);
+     246           0 :   const int new_D = D_ + segment_to_append.D();
+     247             : 
+     248             :   // Create temporary segments to scale polynomials if necessary.
+     249           0 :   Segment current_segment        = *this;
+     250           0 :   Segment segment_to_append_temp = segment_to_append;
+     251             : 
+     252             :   // Scale segment polynomials to the longer segment time.
+     253           0 :   const double new_time = std::max(time_, segment_to_append.getTime());
+     254           0 :   if (time_ < new_time && new_time > 0.0) {
+     255           0 :     for (int d = 0; d < D_; d++) {
+     256           0 :       current_segment[d].scalePolynomialInTime(time_ / new_time);
+     257           0 :     }
+     258           0 :   } else if (segment_to_append.getTime() < new_time && new_time > 0.0) {
+     259           0 :     for (int d = 0; d < segment_to_append.D(); d++) {
+     260           0 :       segment_to_append_temp[d].scalePolynomialInTime(segment_to_append.getTime() / new_time);
+     261             :     }
+     262             :   }
+     263             : 
+     264           0 :   *new_segment = Segment(new_N, new_D);
+     265             : 
+     266           0 :   if (N_ == segment_to_append.N()) {
+     267           0 :     for (int i = 0; i < new_D; i++) {
+     268           0 :       if (i < D_) {
+     269           0 :         (*new_segment)[i] = current_segment[i];
+     270             :       } else {
+     271           0 :         (*new_segment)[i] = segment_to_append_temp[i - D_];
+     272             :       }
+     273             :     }
+     274             :   } else {
+     275           0 :     for (int i = 0; i < new_D; i++) {
+     276           0 :       Polynomial polynomial_to_append(new_N);
+     277           0 :       if (i < D_) {
+     278           0 :         if (!polynomials_[i].getPolynomialWithAppendedCoefficients(new_N, &polynomial_to_append)) {
+     279           0 :           return false;
+     280             :         }
+     281             :       } else {
+     282           0 :         if (!segment_to_append[i - D_].getPolynomialWithAppendedCoefficients(new_N, &polynomial_to_append)) {
+     283           0 :           return false;
+     284             :         }
+     285             :       }
+     286           0 :       (*new_segment)[i] = polynomial_to_append;
+     287             :     }
+     288             :   }
+     289             : 
+     290           0 :   new_segment->setTime(new_time);
+     291           0 :   return true;
+     292             : }
+     293             : 
+     294             : //}
+     295             : 
+     296             : /* offsetSegment() //{ */
+     297             : 
+     298           0 : bool Segment::offsetSegment(const Eigen::VectorXd& A_r_B) {
+     299             : 
+     300           0 :   if (A_r_B.size() < std::min(D_, 3)) {
+     301           0 :     LOG(WARNING) << "Offset vector size smaller than segment dimension.";
+     302           0 :     return false;
+     303             :   }
+     304             : 
+     305             :   // Only translate the first three dimensions.
+     306           0 :   for (size_t i = 0; i < std::min(D_, 3); ++i) {
+     307           0 :     polynomials_[i].offsetPolynomial(A_r_B(i));
+     308             :   }
+     309             : 
+     310           0 :   return true;
+     311             : }
+     312             : 
+     313             : //}
+     314             : 
+     315             : }  // namespace eth_trajectory_generation
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.overview.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.overview.html new file mode 100644 index 0000000000..3324607f03 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.overview.html @@ -0,0 +1,99 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.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 + + +
    + Top

    + Overview +
    + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.png b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/segment.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..ff1ab79381f5ba01991e56e2c9edc44def5539f2 GIT binary patch literal 1496 zcmV;}1tOJ*{xB2v#n?`Xv( zc}RWgio6JHe)G4Km=W~Ah-%<-GA+C~+KvK> z51kJXA0Y8p-rEaJ*1|ZI?+T#5%+dhb$dhy4N8W1FBG9rpz+!#;090`qfOm2LP;YH5 z4jH_kTE!snsH6uM3ymmNKZN-ACOHD_M^4+Z?`uiZdR(Db7y`E*f`FaxBv7xOU!m~5 zuJc2x{eP|Sx8rd}M@5g@o~%GxCxz2DZS|}QWh3x4*0yc?9_QQIz&PYCeFC2(P-#PU zid*3Caf&D1qR7`CUjBr{>`zL|F?~Qv_nfWIO1^_U<4;0ClCR_)1bQkI90Dc1KAc3l z?&oWq&qXGg(KhfhnPhd+W$Tfosl?e&$8g+|(I(^X{3$t&sQ4TeepD_c@42eIP!A6A zoWk|$`R^2-72RX1?<+ig0&51W088v<&9s(E4pb`qdrio8dnfv4HHxGn0Zqz$6cQ3O zSC-TvnWeYIPQcoqF`HV00^mkP*G8$j1H=J)BU;O0!n1!@L$h{YxbCse;gV--%pZ%7 zJ&Ie%yuW^wl^{9rD1fihr9Prmyex2zX_YlsomMW8?#&Z`L2lx_S8}a!r?Us}-Zl!R%VXh`5#QRPzKihUzB_{IzJ5vg?|(^l%< zdw$ba>Z!EeOzanBd_1vtMqHu60{i`7&JA7_VD5MAVJ|gBzEII;(@Oa*)=NSCZ>%B`vA^mX#!Cgp&^`B!NLDisdwG)d}8rB_!^ ztt6CfZY5>$@usv`Q~cdB7B0%qi;sxmnlwCRArgy^TYV1{9}uLHa#}s-0pP!A)*bQj zXjZh4iP9bs+f|dL9V0fIVwbq?Jt+XhEu}xPi51(wC!QF)p9}z&yO-D`Z!nQspI)@> zI=}`%0KfDk_kC|{`Xilc@DpA^2nPk>t)m|H$MoY4P+=OqKiXUvFAqs_wgr#9!9T#` zkg?~~7-&0_J#m$nM>JmYodaF2sEX8DVe-p$aGxsmo0g1cdvaup^R*N&fITl7>Ck^^iA>Qxtm9abv=&n3w z*ixbnkCHynOHor1da7rxB#cszbn}W8(*i0FUg4gUZ)Ee5M|Ql#LUp;SZgaIp1dVwK zeEjB>vRgQ$NAT2~uHZq1H_{=G4<2q+#Z&}p6scEG$)juV`yn1xrnYr2t2ypZM~aT$ z5Rd0K){k#rNF%zeKXy!cLv<|T=C0jZxU%zsvEtm+g8J0R!j;AKkzL%^ECn;Cx|N%L72F>U?i+)X@@KUG0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - timing.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:01110.0 %
    Date:2024-04-09 22:03:33Functions:0300.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::timing::Timer::Stop()0
    eth_trajectory_generation::timing::Timer::Start()0
    eth_trajectory_generation::timing::Timer::Timer(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool)0
    eth_trajectory_generation::timing::Timer::Timer(unsigned long, bool)0
    eth_trajectory_generation::timing::Timer::~Timer()0
    eth_trajectory_generation::timing::Timing::GetMaxSeconds(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::timing::Timing::GetMaxSeconds(unsigned long)0
    eth_trajectory_generation::timing::Timing::GetMinSeconds(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::timing::Timing::GetMinSeconds(unsigned long)0
    eth_trajectory_generation::timing::Timing::GetNumSamples(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::timing::Timing::GetNumSamples(unsigned long)0
    eth_trajectory_generation::timing::Timing::GetMeanSeconds(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::timing::Timing::GetMeanSeconds(unsigned long)0
    eth_trajectory_generation::timing::Timing::GetTotalSeconds(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::timing::Timing::GetTotalSeconds(unsigned long)0
    eth_trajectory_generation::timing::Timing::GetVarianceSeconds(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::timing::Timing::GetVarianceSeconds(unsigned long)0
    eth_trajectory_generation::timing::Timing::SecondsToTimeString[abi:cxx11](double)0
    eth_trajectory_generation::timing::Timing::GetHz(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::timing::Timing::GetHz(unsigned long)0
    eth_trajectory_generation::timing::Timing::Print[abi:cxx11]()0
    eth_trajectory_generation::timing::Timing::Print(std::basic_ostream<char, std::char_traits<char> >&)0
    eth_trajectory_generation::timing::Timing::Reset()0
    eth_trajectory_generation::timing::Timing::GetTag[abi:cxx11](unsigned long)0
    eth_trajectory_generation::timing::Timing::AddTime(unsigned long, double)0
    eth_trajectory_generation::timing::Timing::Instance()0
    eth_trajectory_generation::timing::Timing::GetHandle(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::timing::Timing::Timing()0
    eth_trajectory_generation::timing::Timing::~Timing()0
    eth_trajectory_generation::timing::Timer::IsTiming() const0
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.func.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.func.html new file mode 100644 index 0000000000..6fa82c9777 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.func.html @@ -0,0 +1,200 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - timing.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:01110.0 %
    Date:2024-04-09 22:03:33Functions:0300.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::timing::Timer::Stop()0
    eth_trajectory_generation::timing::Timer::Start()0
    eth_trajectory_generation::timing::Timer::Timer(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool)0
    eth_trajectory_generation::timing::Timer::Timer(unsigned long, bool)0
    eth_trajectory_generation::timing::Timer::~Timer()0
    eth_trajectory_generation::timing::Timing::GetMaxSeconds(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::timing::Timing::GetMaxSeconds(unsigned long)0
    eth_trajectory_generation::timing::Timing::GetMinSeconds(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::timing::Timing::GetMinSeconds(unsigned long)0
    eth_trajectory_generation::timing::Timing::GetNumSamples(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::timing::Timing::GetNumSamples(unsigned long)0
    eth_trajectory_generation::timing::Timing::GetMeanSeconds(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::timing::Timing::GetMeanSeconds(unsigned long)0
    eth_trajectory_generation::timing::Timing::GetTotalSeconds(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::timing::Timing::GetTotalSeconds(unsigned long)0
    eth_trajectory_generation::timing::Timing::GetVarianceSeconds(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::timing::Timing::GetVarianceSeconds(unsigned long)0
    eth_trajectory_generation::timing::Timing::SecondsToTimeString[abi:cxx11](double)0
    eth_trajectory_generation::timing::Timing::GetHz(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::timing::Timing::GetHz(unsigned long)0
    eth_trajectory_generation::timing::Timing::Print[abi:cxx11]()0
    eth_trajectory_generation::timing::Timing::Print(std::basic_ostream<char, std::char_traits<char> >&)0
    eth_trajectory_generation::timing::Timing::Reset()0
    eth_trajectory_generation::timing::Timing::GetTag[abi:cxx11](unsigned long)0
    eth_trajectory_generation::timing::Timing::AddTime(unsigned long, double)0
    eth_trajectory_generation::timing::Timing::Instance()0
    eth_trajectory_generation::timing::Timing::GetHandle(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)0
    eth_trajectory_generation::timing::Timing::Timing()0
    eth_trajectory_generation::timing::Timing::~Timing()0
    eth_trajectory_generation::timing::Timer::IsTiming() const0
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.frameset.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.frameset.html new file mode 100644 index 0000000000..cacbf88688 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.html new file mode 100644 index 0000000000..ba476316f3 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.html @@ -0,0 +1,415 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - timing.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:01110.0 %
    Date:2024-04-09 22:03:33Functions:0300.0 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : /* Adapted from Paul Furgale Schweizer Messer sm_timing*/
+      22             : 
+      23             : #include <math.h>
+      24             : #include <stdio.h>
+      25             : #include <algorithm>
+      26             : #include <ostream>
+      27             : #include <sstream>
+      28             : #include <string>
+      29             : 
+      30             : #include <eth_trajectory_generation/timing.h>
+      31             : 
+      32             : namespace eth_trajectory_generation
+      33             : {
+      34             : namespace timing
+      35             : {
+      36             : 
+      37           0 : Timing& Timing::Instance() {
+      38           0 :   static Timing t;
+      39           0 :   return t;
+      40             : }
+      41             : 
+      42           0 : Timing::Timing() : max_tag_length_(0) {
+      43           0 : }
+      44             : 
+      45           0 : Timing::~Timing() {
+      46           0 : }
+      47             : 
+      48             : /* GetHandle() //{ */
+      49             : 
+      50             : // Static functions to query the timers:
+      51           0 : size_t Timing::GetHandle(std::string const& tag) {
+      52             :   // Search for an existing tag.
+      53           0 :   map_t::iterator i = Instance().tag_map_.find(tag);
+      54           0 :   if (i == Instance().tag_map_.end()) {
+      55             :     // If it is not there, create a tag.
+      56           0 :     size_t handle            = Instance().timers_.size();
+      57           0 :     Instance().tag_map_[tag] = handle;
+      58           0 :     Instance().timers_.push_back(TimerMapValue());
+      59             :     // Track the maximum tag length to help printing a table of timing values
+      60             :     // later.
+      61           0 :     Instance().max_tag_length_ = std::max(Instance().max_tag_length_, tag.size());
+      62           0 :     return handle;
+      63             :   } else {
+      64           0 :     return i->second;
+      65             :   }
+      66             : }
+      67             : 
+      68             : //}
+      69             : 
+      70             : /* GetTag() //{ */
+      71             : 
+      72           0 : std::string Timing::GetTag(size_t handle) {
+      73           0 :   std::string tag;
+      74             : 
+      75             :   // Perform a linear search for the tag.
+      76           0 :   for (typename map_t::value_type current_tag : Instance().tag_map_) {
+      77           0 :     if (current_tag.second == handle) {
+      78           0 :       return current_tag.first;
+      79             :     }
+      80             :   }
+      81           0 :   return tag;
+      82             : }
+      83             : 
+      84             : //}
+      85             : 
+      86             : /* Timer() //{ */
+      87             : 
+      88             : // Class functions used for timing.
+      89           0 : Timer::Timer(size_t handle, bool constructStopped) : timing_(false), handle_(handle) {
+      90           0 :   if (!constructStopped)
+      91           0 :     Start();
+      92           0 : }
+      93             : 
+      94             : //}
+      95             : 
+      96             : /* Timer() //{ */
+      97             : 
+      98           0 : Timer::Timer(std::string const& tag, bool constructStopped) : timing_(false), handle_(Timing::GetHandle(tag)) {
+      99           0 :   if (!constructStopped)
+     100           0 :     Start();
+     101           0 : }
+     102             : 
+     103             : //}
+     104             : 
+     105             : /* ~Timer() //{ */
+     106             : 
+     107           0 : Timer::~Timer() {
+     108           0 :   if (IsTiming())
+     109           0 :     Stop();
+     110           0 : }
+     111             : 
+     112             : //}
+     113             : 
+     114             : /* Start() //{ */
+     115             : 
+     116           0 : void Timer::Start() {
+     117           0 :   timing_ = true;
+     118           0 :   time_   = std::chrono::system_clock::now();
+     119           0 : }
+     120             : 
+     121             : //}
+     122             : 
+     123             : /* Stop() //{ */
+     124             : 
+     125           0 : void Timer::Stop() {
+     126           0 :   std::chrono::time_point<std::chrono::system_clock> now = std::chrono::system_clock::now();
+     127           0 :   double                                             dt  = std::chrono::duration<double>(now - time_).count();
+     128             : 
+     129           0 :   Timing::Instance().AddTime(handle_, dt);
+     130           0 :   timing_ = false;
+     131           0 : }
+     132             : 
+     133             : //}
+     134             : 
+     135             : /* IsTiming() //{ */
+     136             : 
+     137           0 : bool Timer::IsTiming() const {
+     138           0 :   return timing_;
+     139             : }
+     140             : 
+     141             : //}
+     142             : 
+     143             : /* AddTime() //{ */
+     144             : 
+     145           0 : void Timing::AddTime(size_t handle, double seconds) {
+     146           0 :   timers_[handle].acc_.Add(seconds);
+     147           0 : }
+     148             : 
+     149             : //}
+     150             : 
+     151             : /* GetTotalSeconds() //{ */
+     152             : 
+     153           0 : double Timing::GetTotalSeconds(size_t handle) {
+     154           0 :   return Instance().timers_[handle].acc_.Sum();
+     155             : }
+     156             : 
+     157             : //}
+     158             : 
+     159             : /* GetTotalSeconds() //{ */
+     160             : 
+     161           0 : double Timing::GetTotalSeconds(std::string const& tag) {
+     162           0 :   return GetTotalSeconds(GetHandle(tag));
+     163             : }
+     164             : 
+     165             : //}
+     166             : 
+     167             : /* GetMeanSeconds() //{ */
+     168             : 
+     169           0 : double Timing::GetMeanSeconds(size_t handle) {
+     170           0 :   return Instance().timers_[handle].acc_.Mean();
+     171             : }
+     172             : 
+     173             : //}
+     174             : 
+     175             : /* GetMeanSeconds() //{ */
+     176             : 
+     177           0 : double Timing::GetMeanSeconds(std::string const& tag) {
+     178           0 :   return GetMeanSeconds(GetHandle(tag));
+     179             : }
+     180             : 
+     181             : //}
+     182             : 
+     183             : /* GetNumSamples() //{ */
+     184             : 
+     185           0 : size_t Timing::GetNumSamples(size_t handle) {
+     186           0 :   return Instance().timers_[handle].acc_.TotalSamples();
+     187             : }
+     188             : 
+     189             : //}
+     190             : 
+     191             : /* GetNumSamples() //{ */
+     192             : 
+     193           0 : size_t Timing::GetNumSamples(std::string const& tag) {
+     194           0 :   return GetNumSamples(GetHandle(tag));
+     195             : }
+     196             : 
+     197             : //}
+     198             : 
+     199             : /* GetVarianceSeconds() //{ */
+     200             : 
+     201           0 : double Timing::GetVarianceSeconds(size_t handle) {
+     202           0 :   return Instance().timers_[handle].acc_.LazyVariance();
+     203             : }
+     204             : 
+     205             : //}
+     206             : 
+     207             : /* GetVarianceSeconds() //{ */
+     208             : 
+     209           0 : double Timing::GetVarianceSeconds(std::string const& tag) {
+     210           0 :   return GetVarianceSeconds(GetHandle(tag));
+     211             : }
+     212             : 
+     213             : //}
+     214             : 
+     215             : /* GetMinSeconds() //{ */
+     216             : 
+     217           0 : double Timing::GetMinSeconds(size_t handle) {
+     218           0 :   return Instance().timers_[handle].acc_.Min();
+     219             : }
+     220             : 
+     221             : //}
+     222             : 
+     223             : /* GetMinSeconds() //{ */
+     224             : 
+     225           0 : double Timing::GetMinSeconds(std::string const& tag) {
+     226           0 :   return GetMinSeconds(GetHandle(tag));
+     227             : }
+     228             : 
+     229             : //}
+     230             : 
+     231             : /* GetMaxSeconds() //{ */
+     232             : 
+     233           0 : double Timing::GetMaxSeconds(size_t handle) {
+     234           0 :   return Instance().timers_[handle].acc_.Max();
+     235             : }
+     236             : 
+     237             : //}
+     238             : 
+     239             : /* GetMaxSeconds() //{ */
+     240             : 
+     241           0 : double Timing::GetMaxSeconds(std::string const& tag) {
+     242           0 :   return GetMaxSeconds(GetHandle(tag));
+     243             : }
+     244             : 
+     245             : //}
+     246             : 
+     247             : /* GetHz() //{ */
+     248             : 
+     249           0 : double Timing::GetHz(size_t handle) {
+     250           0 :   return 1.0 / Instance().timers_[handle].acc_.RollingMean();
+     251             : }
+     252             : 
+     253             : //}
+     254             : 
+     255             : /* GetHz() //{ */
+     256             : 
+     257           0 : double Timing::GetHz(std::string const& tag) {
+     258           0 :   return GetHz(GetHandle(tag));
+     259             : }
+     260             : 
+     261             : //}
+     262             : 
+     263             : /* SecondsToTimeString() //{ */
+     264             : 
+     265           0 : std::string Timing::SecondsToTimeString(double seconds) {
+     266             :   char buffer[256];
+     267           0 :   snprintf(buffer, sizeof(buffer), "%09.6f", seconds);
+     268           0 :   return buffer;
+     269             : }
+     270             : 
+     271             : //}
+     272             : 
+     273             : /* Print() //{ */
+     274             : 
+     275           0 : void Timing::Print(std::ostream& out) {
+     276           0 :   map_t& tagMap = Instance().tag_map_;
+     277             : 
+     278           0 :   if (tagMap.empty()) {
+     279           0 :     return;
+     280             :   }
+     281             : 
+     282           0 :   out << "SM Timing\n";
+     283           0 :   out << "-----------\n";
+     284           0 :   for (typename map_t::value_type t : tagMap) {
+     285           0 :     size_t i = t.second;
+     286           0 :     out.width((std::streamsize)Instance().max_tag_length_);
+     287           0 :     out.setf(std::ios::left, std::ios::adjustfield);
+     288           0 :     out << t.first << "\t";
+     289           0 :     out.width(7);
+     290             : 
+     291           0 :     out.setf(std::ios::right, std::ios::adjustfield);
+     292           0 :     out << GetNumSamples(i) << "\t";
+     293           0 :     if (GetNumSamples(i) > 0) {
+     294           0 :       out << SecondsToTimeString(GetTotalSeconds(i)) << "\t";
+     295           0 :       double meansec = GetMeanSeconds(i);
+     296           0 :       double stddev  = sqrt(GetVarianceSeconds(i));
+     297           0 :       out << "(" << SecondsToTimeString(meansec) << " +- ";
+     298           0 :       out << SecondsToTimeString(stddev) << ")\t";
+     299             : 
+     300           0 :       double minsec = GetMinSeconds(i);
+     301           0 :       double maxsec = GetMaxSeconds(i);
+     302             : 
+     303             :       // The min or max are out of bounds.
+     304           0 :       out << "[" << SecondsToTimeString(minsec) << "," << SecondsToTimeString(maxsec) << "]";
+     305             :     }
+     306           0 :     out << std::endl;
+     307             :   }
+     308             : }
+     309             : 
+     310             : //}
+     311             : 
+     312             : /* Print() //{ */
+     313             : 
+     314           0 : std::string Timing::Print() {
+     315           0 :   std::stringstream ss;
+     316           0 :   Print(ss);
+     317           0 :   return ss.str();
+     318             : }
+     319             : 
+     320             : //}
+     321             : 
+     322             : /* Reset() //{ */
+     323             : 
+     324           0 : void Timing::Reset() {
+     325           0 :   Instance().tag_map_.clear();
+     326           0 : }
+     327             : 
+     328             : //}
+     329             : 
+     330             : }  // namespace timing
+     331             : }  // namespace eth_trajectory_generation
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.overview.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.overview.html new file mode 100644 index 0000000000..6ba786c671 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.overview.html @@ -0,0 +1,103 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.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 + + +
    + Top

    + Overview +
    + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.png b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/timing.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..b16739711ddd7d62de5d3b78dc645d2b9592b643 GIT binary patch literal 1088 zcmV-G1i$-Z0lnW9N1_Hw;hw~? zpKb>rwd<7JmSwAAl}o{^1eKU7q)3w&!7y`d(*7LYV{5s^ZW(~E-Fz0~l)84`bCRz*| z4tziMqgHq+U1A!Cm>EP>_6(D7*rq@lfUPMuMimBVYEhN3t7F$iuxe^5VT`uODx^dn zAWuI0RWo!vx4L2F+N8&YYeKLH8&>%2K>%yT7S2prNJH)^G%@5cWHScg8&=l?JX2+Q zpgf0UyfS~9@b#&AzS&cW@HtR1Q0gSY7b{$|DEB$sDW2h(2h;8`k$-j!;Y$Uc37>5F zE^3SGH?qa9(*toE+SEWc;a!*b9w>=oxNRNAYwCD>Olt+xRvv^#VB71iHdzyaM zgdV%-etE1STQ{+d@`%!N6pzc6nvNafbqcA+md}^RQF=sq{PJ9ZKkJR_%&N#Hc*ITt z{zpA-o5ji_N$0_**TXo>*zu}&l5`K(l_8hw;8mUuzn`&!fBRs?9F&|Zq zk9_ziXY6tlc2mAiXI|<8z)AE+U>OXd3<`Vo=^59(q){N%3<)g#-dhZ_(c^QrWRY?B z$!pCR%aqZ+VXqi>SG2{VJb{*)uS+r5?yRAqC}1hd5GjixYMK?+AV4oP({^mCN=tN% z8aem+gzzr1CuKVfAg}9?(C%0cF32iGh11jhieE95Y5a2)Z0qY%QuztZRbH>nG?Cg~ z&J=K(9M!w9PtNz-qsj$5GI2YOD#acD@6qIST>%_c`Tqk2v?FGlkStaJ0000 + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - trajectory.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:18031457.3 %
    Date:2024-04-09 22:03:33Functions:112347.8 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::Trajectory::offsetTrajectory(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)0
    eth_trajectory_generation::Trajectory::scaleSegmentTimes(double)0
    eth_trajectory_generation::Trajectory::getVertices(int, std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> >*) const0
    eth_trajectory_generation::Trajectory::getVertices(int, int, std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> >*, std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> >*) const0
    eth_trajectory_generation::Trajectory::getGoalVertex(int) const0
    eth_trajectory_generation::Trajectory::getStartVertex(int) const0
    eth_trajectory_generation::Trajectory::addTrajectories(std::vector<eth_trajectory_generation::Trajectory, std::allocator<eth_trajectory_generation::Trajectory> > const&, eth_trajectory_generation::Trajectory*) const0
    eth_trajectory_generation::Trajectory::getVertexAtTime(double, int) const0
    eth_trajectory_generation::Trajectory::getTrajectoryWithSingleDimension(int) const0
    eth_trajectory_generation::Trajectory::getTrajectoryWithAppendedDimension(eth_trajectory_generation::Trajectory const&, eth_trajectory_generation::Trajectory*) const0
    eth_trajectory_generation::Trajectory::evaluate(double, int) const0
    eth_trajectory_generation::Trajectory::operator==(eth_trajectory_generation::Trajectory const&) const0
    eth_trajectory_generation::Trajectory::scaleSegmentTimesToMeetConstraints(double, double, double, double, double, double, double, double, double)16
    eth_trajectory_generation::Trajectory::getSegmentTimes() const16
    eth_trajectory_generation::Trajectory::computeMaxDerivativesHeading(double*, double*, double*) const16
    eth_trajectory_generation::Trajectory::computeMaxDerivativesVertical(double*, double*, double*) const16
    eth_trajectory_generation::Trajectory::computeMaxDerivativesHorizontal(double*, double*, double*) const16
    eth_trajectory_generation::Trajectory::evaluateRange(double, double, double, int, std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, std::allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >*, std::vector<double, std::allocator<double> >*) const55
    eth_trajectory_generation::Trajectory::computeMinMaxMagnitude(int, std::vector<int, std::allocator<int> > const&, eth_trajectory_generation::Extremum*, eth_trajectory_generation::Extremum*) const144
    eth_trajectory_generation::Trajectory::computeMaxDerivativesHeading(double*, double*, double*, int) const165
    eth_trajectory_generation::Trajectory::computeMaxDerivativesVertical(double*, double*, double*, int) const165
    eth_trajectory_generation::Trajectory::computeMaxDerivativesHorizontal(double*, double*, double*, int) const165
    eth_trajectory_generation::Trajectory::computeMinMaxMagnitude(int, std::vector<int, std::allocator<int> > const&, eth_trajectory_generation::Extremum*, eth_trajectory_generation::Extremum*, int) const1485
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.func.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.func.html new file mode 100644 index 0000000000..d0ca47a526 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.func.html @@ -0,0 +1,172 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - trajectory.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:18031457.3 %
    Date:2024-04-09 22:03:33Functions:112347.8 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::Trajectory::offsetTrajectory(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)0
    eth_trajectory_generation::Trajectory::scaleSegmentTimes(double)0
    eth_trajectory_generation::Trajectory::scaleSegmentTimesToMeetConstraints(double, double, double, double, double, double, double, double, double)16
    eth_trajectory_generation::Trajectory::getVertices(int, std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> >*) const0
    eth_trajectory_generation::Trajectory::getVertices(int, int, std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> >*, std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> >*) const0
    eth_trajectory_generation::Trajectory::evaluateRange(double, double, double, int, std::vector<Eigen::Matrix<double, -1, 1, 0, -1, 1>, std::allocator<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >*, std::vector<double, std::allocator<double> >*) const55
    eth_trajectory_generation::Trajectory::getGoalVertex(int) const0
    eth_trajectory_generation::Trajectory::getStartVertex(int) const0
    eth_trajectory_generation::Trajectory::addTrajectories(std::vector<eth_trajectory_generation::Trajectory, std::allocator<eth_trajectory_generation::Trajectory> > const&, eth_trajectory_generation::Trajectory*) const0
    eth_trajectory_generation::Trajectory::getSegmentTimes() const16
    eth_trajectory_generation::Trajectory::getVertexAtTime(double, int) const0
    eth_trajectory_generation::Trajectory::computeMinMaxMagnitude(int, std::vector<int, std::allocator<int> > const&, eth_trajectory_generation::Extremum*, eth_trajectory_generation::Extremum*) const144
    eth_trajectory_generation::Trajectory::computeMinMaxMagnitude(int, std::vector<int, std::allocator<int> > const&, eth_trajectory_generation::Extremum*, eth_trajectory_generation::Extremum*, int) const1485
    eth_trajectory_generation::Trajectory::computeMaxDerivativesHeading(double*, double*, double*) const16
    eth_trajectory_generation::Trajectory::computeMaxDerivativesHeading(double*, double*, double*, int) const165
    eth_trajectory_generation::Trajectory::computeMaxDerivativesVertical(double*, double*, double*) const16
    eth_trajectory_generation::Trajectory::computeMaxDerivativesVertical(double*, double*, double*, int) const165
    eth_trajectory_generation::Trajectory::computeMaxDerivativesHorizontal(double*, double*, double*) const16
    eth_trajectory_generation::Trajectory::computeMaxDerivativesHorizontal(double*, double*, double*, int) const165
    eth_trajectory_generation::Trajectory::getTrajectoryWithSingleDimension(int) const0
    eth_trajectory_generation::Trajectory::getTrajectoryWithAppendedDimension(eth_trajectory_generation::Trajectory const&, eth_trajectory_generation::Trajectory*) const0
    eth_trajectory_generation::Trajectory::evaluate(double, int) const0
    eth_trajectory_generation::Trajectory::operator==(eth_trajectory_generation::Trajectory const&) const0
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.frameset.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.frameset.html new file mode 100644 index 0000000000..794554bfde --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.html new file mode 100644 index 0000000000..dd9b7e611a --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.html @@ -0,0 +1,780 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - trajectory.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:18031457.3 %
    Date:2024-04-09 22:03:33Functions:112347.8 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #include <eth_trajectory_generation/trajectory.h>
+      22             : #include <limits>
+      23             : 
+      24             : // fixes error due to std::iota (has been introduced in c++ standard lately
+      25             : // and may cause compilation errors depending on compiler)
+      26             : #if __cplusplus <= 199711L
+      27             : #include <algorithm>
+      28             : #else
+      29             : #include <numeric>
+      30             : #endif
+      31             : 
+      32             : namespace eth_trajectory_generation
+      33             : {
+      34             : 
+      35             : /* operator==(const Trajectory& rhs) //{ */
+      36             : 
+      37           0 : bool Trajectory::operator==(const Trajectory& rhs) const {
+      38           0 :   if (segments_.size() != rhs.segments_.size()) {
+      39             :     // Different number of segments.
+      40           0 :     return false;
+      41             :   } else {
+      42           0 :     for (int i = 0; i < K(); i++) {
+      43           0 :       if (segments_ != rhs.segments_) {
+      44           0 :         return false;
+      45             :       }
+      46             :     }
+      47           0 :     return true;
+      48             :   }
+      49             : }
+      50             : 
+      51             : //}
+      52             : 
+      53             : /* evaluate() //{ */
+      54             : 
+      55           0 : Eigen::VectorXd Trajectory::evaluate(double t, int derivative_order) const {
+      56             : 
+      57           0 :   double accumulated_time = 0.0;
+      58             : 
+      59             :   // Look for the correct segment.
+      60           0 :   size_t i = 0;
+      61           0 :   for (i = 0; i < segments_.size(); ++i) {
+      62           0 :     accumulated_time += segments_[i].getTime();
+      63             :     // |<--t_accumulated -->|
+      64             :     // x----------x---------x
+      65             :     //               ^t_start
+      66             :     //            |chosen it|
+      67             :     // in case t_start falls on a vertex, the iterator right of the vertex is
+      68             :     // chosen, hence accumulated_segment_time_ns > t_start
+      69           0 :     if (accumulated_time > t) {
+      70           0 :       break;
+      71             :     }
+      72             :   }
+      73           0 :   if (t > accumulated_time) {
+      74           0 :     LOG(ERROR) << "Time out of range of the trajectory!";
+      75           0 :     return Eigen::VectorXd::Zero(D(), 1);
+      76             :   }
+      77             : 
+      78             :   // Make sure we don't go off the end of the segments (can happen if t is
+      79             :   // equal to trajectory max time).
+      80           0 :   if (i >= segments_.size()) {
+      81           0 :     i = segments_.size() - 1;
+      82             :   }
+      83             :   // Go back to the start of this segment.
+      84           0 :   accumulated_time -= segments_[i].getTime();
+      85             : 
+      86           0 :   return segments_[i].evaluate(t - accumulated_time, derivative_order);
+      87             : }
+      88             : 
+      89             : //}
+      90             : 
+      91             : /* evaluateRange() //{ */
+      92             : 
+      93          55 : void Trajectory::evaluateRange(double t_start, double t_end, double dt, int derivative_order, std::vector<Eigen::VectorXd>* result,
+      94             :                                std::vector<double>* sampling_times) const {
+      95          55 :   const size_t expected_number_of_samples = (t_end - t_start) / dt + 1;
+      96             : 
+      97          55 :   result->clear();
+      98          55 :   result->reserve(expected_number_of_samples);
+      99             : 
+     100          55 :   if (sampling_times != nullptr) {
+     101           0 :     sampling_times->clear();
+     102           0 :     sampling_times->reserve(expected_number_of_samples);
+     103             :   }
+     104             : 
+     105          55 :   double accumulated_time = 0.0;
+     106             : 
+     107             :   // Look for the correct segment to start.
+     108          55 :   size_t i = 0;
+     109          55 :   for (i = 0; i < segments_.size(); ++i) {
+     110          55 :     accumulated_time += segments_[i].getTime();
+     111             :     // |<--t_accumulated -->|
+     112             :     // x----------x---------x
+     113             :     //               ^t_start
+     114             :     //            |chosen it|
+     115             :     // in case t_start falls on a vertex, the iterator right of the vertex is
+     116             :     // chosen, hence accumulated_segment_time_ns > t_start
+     117          55 :     if (accumulated_time > t_start) {
+     118          55 :       break;
+     119             :     }
+     120             :   }
+     121          55 :   if (t_start > accumulated_time) {
+     122           0 :     LOG(ERROR) << "Start time out of range of the trajectory!";
+     123           0 :     return;
+     124             :   }
+     125             : 
+     126             :   // Go back to the start of this segment.
+     127          55 :   accumulated_time -= segments_[i].getTime();
+     128          55 :   double time_in_segment = t_start - accumulated_time;
+     129             : 
+     130             :   // Get all the samples, incrementing the segments as we go.
+     131        7530 :   while (accumulated_time < t_end) {
+     132        7475 :     if (time_in_segment > segments_[i].getTime()) {
+     133         435 :       time_in_segment = time_in_segment - segments_[i].getTime();
+     134         435 :       i++;
+     135             :       // Make sure we don't access segments that don't exist!
+     136         435 :       if (i >= segments_.size()) {
+     137           0 :         break;
+     138             :       }
+     139         435 :       continue;
+     140             :     }
+     141             : 
+     142        7040 :     result->push_back(segments_[i].evaluate(time_in_segment, derivative_order));
+     143             : 
+     144        7040 :     if (sampling_times != nullptr) {
+     145           0 :       sampling_times->push_back(accumulated_time);
+     146             :     }
+     147             : 
+     148        7040 :     time_in_segment += dt;
+     149        7040 :     accumulated_time += dt;
+     150             :   }
+     151             : }
+     152             : 
+     153             : //}
+     154             : 
+     155             : /* getTrajectoryWithSingleDimension() //{ */
+     156             : 
+     157           0 : Trajectory Trajectory::getTrajectoryWithSingleDimension(int dimension) const {
+     158           0 :   CHECK_LT(dimension, D_);
+     159             : 
+     160             :   // Create a new set of segments with just 1 dimension.
+     161           0 :   Segment::Vector segments;
+     162           0 :   segments.reserve(segments_.size());
+     163             : 
+     164           0 :   for (size_t k = 0; k < segments_.size(); ++k) {
+     165           0 :     Segment segment(N_, 1);
+     166           0 :     segment[0] = (segments_[k])[dimension];
+     167           0 :     segments.push_back(segment);
+     168             :   }
+     169             : 
+     170           0 :   Trajectory traj;
+     171           0 :   traj.setSegments(segments);
+     172           0 :   return traj;
+     173             : }
+     174             : 
+     175             : //}
+     176             : 
+     177             : /* getTrajectoryWithAppendedDimension() //{ */
+     178             : 
+     179           0 : bool Trajectory::getTrajectoryWithAppendedDimension(const Trajectory& trajectory_to_append, Trajectory* new_trajectory) const {
+     180             :   // Handle the case of one of the trajectories being empty.
+     181           0 :   if (N_ == 0 || D_ == 0) {
+     182           0 :     *new_trajectory = trajectory_to_append;
+     183           0 :     return true;
+     184             :   }
+     185           0 :   if (trajectory_to_append.N() == 0 || trajectory_to_append.D() == 0) {
+     186           0 :     *new_trajectory = *this;
+     187           0 :     return true;
+     188             :   }
+     189           0 :   CHECK_EQ(static_cast<int>(segments_.size()), trajectory_to_append.K());
+     190             : 
+     191             :   // Create a new set of segments with all of the dimensions.
+     192           0 :   Segment::Vector segments;
+     193           0 :   segments.reserve(segments_.size());
+     194             : 
+     195           0 :   for (size_t k = 0; k < segments_.size(); ++k) {
+     196           0 :     Segment new_segment(0, 0);
+     197           0 :     if (!segments_[k].getSegmentWithAppendedDimension(trajectory_to_append.segments()[k], &new_segment)) {
+     198           0 :       return false;
+     199             :     }
+     200           0 :     segments.push_back(new_segment);
+     201             :   }
+     202             : 
+     203           0 :   new_trajectory->setSegments(segments);
+     204           0 :   return true;
+     205             : }
+     206             : 
+     207             : //}
+     208             : 
+     209             : /* computeMinMaxMagnitude() //{ */
+     210             : 
+     211        1485 : bool Trajectory::computeMinMaxMagnitude(int derivative, const std::vector<int>& dimensions, Extremum* minimum, Extremum* maximum, int seg) const {
+     212             : 
+     213        1485 :   CHECK_NOTNULL(minimum);
+     214        1485 :   CHECK_NOTNULL(maximum);
+     215             : 
+     216        1485 :   minimum->value = std::numeric_limits<double>::max();
+     217        1485 :   maximum->value = std::numeric_limits<double>::lowest();
+     218             : 
+     219             :   // Compute candidates.
+     220        2970 :   std::vector<Extremum> candidates;
+     221        1485 :   if (!segments_[seg].computeMinMaxMagnitudeCandidates(derivative, 0.0, segments_[seg].getTime(), dimensions, &candidates)) {
+     222           0 :     return false;
+     223             :   }
+     224             :   // Evaluate candidates.
+     225        1485 :   Extremum minimum_candidate, maximum_candidate;
+     226        1485 :   if (!segments_[seg].selectMinMaxMagnitudeFromCandidates(derivative, 0.0, segments_[seg].getTime(), dimensions, candidates, &minimum_candidate,
+     227             :                                                           &maximum_candidate)) {
+     228           0 :     return false;
+     229             :   }
+     230             :   // Select minimum / maximum.
+     231        1485 :   if (minimum_candidate < *minimum) {
+     232        1485 :     *minimum             = minimum_candidate;
+     233        1485 :     minimum->segment_idx = static_cast<int>(seg);
+     234             :   }
+     235        1485 :   if (maximum_candidate > *maximum) {
+     236        1485 :     *maximum             = maximum_candidate;
+     237        1485 :     maximum->segment_idx = static_cast<int>(seg);
+     238             :   }
+     239             : 
+     240        1485 :   return true;
+     241             : }
+     242             : 
+     243             : //}
+     244             : 
+     245             : /* computeMinMaxMagnitude() //{ */
+     246             : 
+     247         144 : bool Trajectory::computeMinMaxMagnitude(int derivative, const std::vector<int>& dimensions, Extremum* minimum, Extremum* maximum) const {
+     248             : 
+     249         144 :   CHECK_NOTNULL(minimum);
+     250         144 :   CHECK_NOTNULL(maximum);
+     251             : 
+     252         144 :   minimum->value = std::numeric_limits<double>::max();
+     253         144 :   maximum->value = std::numeric_limits<double>::lowest();
+     254             : 
+     255             :   // For all segments in the trajectory:
+     256        1629 :   for (size_t segment_idx = 0; segment_idx < segments_.size(); segment_idx++) {
+     257             : 
+     258             :     // Compute candidates.
+     259        1485 :     std::vector<Extremum> candidates;
+     260        1485 :     if (!segments_[segment_idx].computeMinMaxMagnitudeCandidates(derivative, 0.0, segments_[segment_idx].getTime(), dimensions, &candidates)) {
+     261           0 :       return false;
+     262             :     }
+     263             :     // Evaluate candidates.
+     264        1485 :     Extremum minimum_candidate, maximum_candidate;
+     265        1485 :     if (!segments_[segment_idx].selectMinMaxMagnitudeFromCandidates(derivative, 0.0, segments_[segment_idx].getTime(), dimensions, candidates,
+     266             :                                                                     &minimum_candidate, &maximum_candidate)) {
+     267           0 :       return false;
+     268             :     }
+     269             :     // Select minimum / maximum.
+     270        1485 :     if (minimum_candidate < *minimum) {
+     271         152 :       *minimum             = minimum_candidate;
+     272         152 :       minimum->segment_idx = static_cast<int>(segment_idx);
+     273             :     }
+     274        1485 :     if (maximum_candidate > *maximum) {
+     275         354 :       *maximum             = maximum_candidate;
+     276         354 :       maximum->segment_idx = static_cast<int>(segment_idx);
+     277             :     }
+     278             :   }
+     279         144 :   return true;
+     280             : }
+     281             : 
+     282             : //}
+     283             : 
+     284             : /* getSegmentTimes() //{ */
+     285             : 
+     286          16 : std::vector<double> Trajectory::getSegmentTimes() const {
+     287          16 :   std::vector<double> segment_times(segments_.size());
+     288         181 :   for (size_t i = 0; i < segment_times.size(); ++i) {
+     289         165 :     segment_times[i] = segments_[i].getTime();
+     290             :   }
+     291          16 :   return segment_times;
+     292             : }
+     293             : 
+     294             : //}
+     295             : 
+     296             : /* addTrajectories() //{ */
+     297             : 
+     298           0 : bool Trajectory::addTrajectories(const std::vector<Trajectory>& trajectories, Trajectory* merged) const {
+     299           0 :   CHECK_NOTNULL(merged);
+     300           0 :   merged->clear();
+     301           0 :   *merged = *this;
+     302             : 
+     303           0 :   for (const Trajectory& t : trajectories) {
+     304             :     // Check dimensions and coefficients.
+     305             :     // TODO(rikba): Allow different number of coefficients.
+     306           0 :     if (t.D() != D_ || t.N() != N_) {
+     307           0 :       LOG(WARNING) << "Dimension to append: " << t.D() << " this dimension: " << D_;
+     308           0 :       LOG(WARNING) << "Number of coefficients to append: " << t.N() << " this number of coefficients: " << N_;
+     309           0 :       return false;
+     310             :     }
+     311             :     // Add segments.
+     312           0 :     Segment::Vector segments;
+     313           0 :     t.getSegments(&segments);
+     314           0 :     merged->addSegments(segments);
+     315             :   }
+     316             : 
+     317           0 :   return true;
+     318             : }
+     319             : 
+     320             : //}
+     321             : 
+     322             : /* offsetTrajectory() //{ */
+     323             : 
+     324           0 : bool Trajectory::offsetTrajectory(const Eigen::VectorXd& A_r_B) {
+     325           0 :   if (A_r_B.size() < std::min(D_, 3)) {
+     326           0 :     LOG(WARNING) << "Offset vector size smaller than trajectory dimension.";
+     327           0 :     return false;
+     328             :   }
+     329             : 
+     330           0 :   for (Segment& s : segments_) {
+     331             :     // Returns false if dimension check fails at segment level.
+     332           0 :     if (!s.offsetSegment(A_r_B))
+     333           0 :       return false;
+     334             :   }
+     335             : 
+     336           0 :   return true;
+     337             : }
+     338             : 
+     339             : //}
+     340             : 
+     341             : /* offsetTrajectory() //{ */
+     342             : 
+     343           0 : Vertex Trajectory::getVertexAtTime(double t, int max_derivative_order) const {
+     344           0 :   Vertex v(D_);
+     345           0 :   for (int i = 0; i <= max_derivative_order; i++) {
+     346           0 :     v.addConstraint(i, evaluate(t, i));
+     347             :   }
+     348           0 :   return v;
+     349             : }
+     350             : 
+     351             : //}
+     352             : 
+     353             : /* getStartVertex() //{ */
+     354             : 
+     355           0 : Vertex Trajectory::getStartVertex(int max_derivative_order) const {
+     356           0 :   return getVertexAtTime(0.0, max_derivative_order);
+     357             : }
+     358             : 
+     359             : //}
+     360             : 
+     361             : /* getGoalVertex() //{ */
+     362             : 
+     363           0 : Vertex Trajectory::getGoalVertex(int max_derivative_order) const {
+     364           0 :   return getVertexAtTime(max_time_, max_derivative_order);
+     365             : }
+     366             : 
+     367             : //}
+     368             : 
+     369             : /* getVertices() //{ */
+     370             : 
+     371           0 : bool Trajectory::getVertices(int max_derivative_order_pos, int max_derivative_order_yaw, Vertex::Vector* pos_vertices, Vertex::Vector* yaw_vertices) const {
+     372           0 :   CHECK_NOTNULL(pos_vertices);
+     373           0 :   CHECK_NOTNULL(yaw_vertices);
+     374           0 :   const std::vector<size_t> kPosDimensions      = {0, 1, 2};
+     375           0 :   const std::vector<size_t> kYawDimensions      = {3};
+     376           0 :   const int                 kMaxDerivativeOrder = std::max(max_derivative_order_pos, max_derivative_order_yaw);
+     377           0 :   pos_vertices->resize(segments_.size() + 1, Vertex(3));
+     378           0 :   yaw_vertices->resize(segments_.size() + 1, Vertex(1));
+     379             : 
+     380             :   // Start vertex.
+     381           0 :   Vertex temp_vertex(4);
+     382           0 :   temp_vertex = getStartVertex(kMaxDerivativeOrder);
+     383           0 :   if (!temp_vertex.getSubdimension(kPosDimensions, max_derivative_order_pos, &pos_vertices->front()))
+     384           0 :     return false;
+     385           0 :   if (!temp_vertex.getSubdimension(kYawDimensions, max_derivative_order_yaw, &yaw_vertices->front()))
+     386           0 :     return false;
+     387             : 
+     388           0 :   double t = 0.0;
+     389           0 :   for (size_t i = 0; i < segments_.size(); ++i) {
+     390           0 :     t += segments_[i].getTime();
+     391           0 :     temp_vertex = getVertexAtTime(t, kMaxDerivativeOrder);
+     392           0 :     if (!temp_vertex.getSubdimension(kPosDimensions, max_derivative_order_pos, &(*pos_vertices)[i + 1]))
+     393           0 :       return false;
+     394           0 :     if (!temp_vertex.getSubdimension(kYawDimensions, max_derivative_order_yaw, &(*yaw_vertices)[i + 1]))
+     395           0 :       return false;
+     396             :   }
+     397           0 :   return true;
+     398             : }
+     399             : 
+     400             : //}
+     401             : 
+     402             : /* getVertices() //{ */
+     403             : 
+     404           0 : bool Trajectory::getVertices(int max_derivative_order, Vertex::Vector* vertices) const {
+     405           0 :   CHECK_NOTNULL(vertices);
+     406           0 :   vertices->resize(segments_.size() + 1, D_);
+     407           0 :   vertices->front() = getStartVertex(max_derivative_order);
+     408             : 
+     409           0 :   double t = 0.0;
+     410           0 :   for (size_t i = 0; i < segments_.size(); ++i) {
+     411           0 :     t += segments_[i].getTime();
+     412           0 :     (*vertices)[i + 1] = getVertexAtTime(t, max_derivative_order);
+     413             :   }
+     414           0 :   return true;
+     415             : }
+     416             : 
+     417             : //}
+     418             : 
+     419             : /* computeMaxDerivativesHorizontal() //{ */
+     420             : 
+     421             : // compute max velocity, acceleration and jerk
+     422         165 : bool Trajectory::computeMaxDerivativesHorizontal(double* v_max, double* a_max, double* j_max, int seg) const {
+     423             : 
+     424             :   // not counting the heading dimension, that is going to be solved separately
+     425         165 :   std::vector<int> dimensions;  // Evaluate in whatever dimensions we have.
+     426             : 
+     427         165 :   dimensions.push_back(0);
+     428         165 :   dimensions.push_back(1);
+     429             : 
+     430         165 :   Extremum v_min_traj, v_max_traj, a_min_traj, a_max_traj, j_min_traj, j_max_traj;
+     431             : 
+     432         165 :   bool success = computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::VELOCITY, dimensions, &v_min_traj, &v_max_traj, seg);
+     433         165 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::ACCELERATION, dimensions, &a_min_traj, &a_max_traj, seg);
+     434         165 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::JERK, dimensions, &j_min_traj, &j_max_traj, seg);
+     435             : 
+     436         165 :   *v_max = v_max_traj.value;
+     437         165 :   *a_max = a_max_traj.value;
+     438         165 :   *j_max = j_max_traj.value;
+     439             : 
+     440         330 :   return success;
+     441             : }
+     442             : 
+     443             : //}
+     444             : 
+     445             : /* computeMaxDerivativesHorizontal() //{ */
+     446             : 
+     447             : // compute max velocity, acceleration and jerk
+     448          16 : bool Trajectory::computeMaxDerivativesHorizontal(double* v_max, double* a_max, double* j_max) const {
+     449             : 
+     450             :   // not counting the heading dimension, that is going to be solved separately
+     451          16 :   std::vector<int> dimensions;  // Evaluate in whatever dimensions we have.
+     452             : 
+     453          16 :   dimensions.push_back(0);
+     454          16 :   dimensions.push_back(1);
+     455             : 
+     456          16 :   Extremum v_min_traj, v_max_traj, a_min_traj, a_max_traj, j_min_traj, j_max_traj;
+     457             : 
+     458          16 :   bool success = computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::VELOCITY, dimensions, &v_min_traj, &v_max_traj);
+     459          16 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::ACCELERATION, dimensions, &a_min_traj, &a_max_traj);
+     460          16 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::JERK, dimensions, &j_min_traj, &j_max_traj);
+     461             : 
+     462          16 :   *v_max = v_max_traj.value;
+     463          16 :   *a_max = a_max_traj.value;
+     464          16 :   *j_max = j_max_traj.value;
+     465             : 
+     466          32 :   return success;
+     467             : }
+     468             : 
+     469             : //}
+     470             : 
+     471             : /* computeMaxDerivativesVertical() //{ */
+     472             : 
+     473             : // compute max velocity, acceleration and jerk
+     474         165 : bool Trajectory::computeMaxDerivativesVertical(double* v_max, double* a_max, double* j_max, int seg) const {
+     475             : 
+     476             :   // not counting the heading dimension, that is going to be solved separately
+     477         165 :   std::vector<int> dimensions;  // Evaluate in whatever dimensions we have.
+     478             : 
+     479         165 :   dimensions.push_back(2);
+     480             : 
+     481         165 :   Extremum v_min_traj, v_max_traj, a_min_traj, a_max_traj, j_min_traj, j_max_traj;
+     482             : 
+     483         165 :   bool success = computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::VELOCITY, dimensions, &v_min_traj, &v_max_traj, seg);
+     484         165 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::ACCELERATION, dimensions, &a_min_traj, &a_max_traj, seg);
+     485         165 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::JERK, dimensions, &j_min_traj, &j_max_traj, seg);
+     486             : 
+     487         165 :   *v_max = v_max_traj.value;
+     488         165 :   *a_max = a_max_traj.value;
+     489         165 :   *j_max = j_max_traj.value;
+     490             : 
+     491         330 :   return success;
+     492             : }
+     493             : 
+     494             : //}
+     495             : 
+     496             : /* computeMaxDerivativesVertical() //{ */
+     497             : 
+     498             : // compute max velocity, acceleration and jerk
+     499          16 : bool Trajectory::computeMaxDerivativesVertical(double* v_max, double* a_max, double* j_max) const {
+     500             : 
+     501             :   // not counting the heading dimension, that is going to be solved separately
+     502          16 :   std::vector<int> dimensions;  // Evaluate in whatever dimensions we have.
+     503             : 
+     504          16 :   dimensions.push_back(2);
+     505             : 
+     506          16 :   Extremum v_min_traj, v_max_traj, a_min_traj, a_max_traj, j_min_traj, j_max_traj;
+     507             : 
+     508          16 :   bool success = computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::VELOCITY, dimensions, &v_min_traj, &v_max_traj);
+     509          16 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::ACCELERATION, dimensions, &a_min_traj, &a_max_traj);
+     510          16 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::JERK, dimensions, &j_min_traj, &j_max_traj);
+     511             : 
+     512          16 :   *v_max = v_max_traj.value;
+     513          16 :   *a_max = a_max_traj.value;
+     514          16 :   *j_max = j_max_traj.value;
+     515             : 
+     516          32 :   return success;
+     517             : }
+     518             : 
+     519             : //}
+     520             : 
+     521             : /* computeMaxDerivativesHeading() //{ */
+     522             : 
+     523             : // compute max velocity, acceleration and jerk
+     524         165 : bool Trajectory::computeMaxDerivativesHeading(double* v_max, double* a_max, double* j_max, int seg) const {
+     525             : 
+     526         165 :   std::vector<int> dimensions;  // Evaluate in whatever dimensions we have.
+     527             : 
+     528         165 :   dimensions.push_back(3);  // 3 = heading
+     529             : 
+     530         165 :   Extremum v_min_traj, v_max_traj, a_min_traj, a_max_traj, j_min_traj, j_max_traj;
+     531             : 
+     532         165 :   bool success = computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::VELOCITY, dimensions, &v_min_traj, &v_max_traj, seg);
+     533         165 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::ACCELERATION, dimensions, &a_min_traj, &a_max_traj, seg);
+     534         165 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::JERK, dimensions, &j_min_traj, &j_max_traj, seg);
+     535             : 
+     536         165 :   *v_max = v_max_traj.value;
+     537         165 :   *a_max = a_max_traj.value;
+     538         165 :   *j_max = j_max_traj.value;
+     539             : 
+     540         330 :   return success;
+     541             : }
+     542             : 
+     543             : //}
+     544             : 
+     545             : /* computeMaxDerivativesHeading() //{ */
+     546             : 
+     547             : // compute max velocity, acceleration and jerk
+     548          16 : bool Trajectory::computeMaxDerivativesHeading(double* v_max, double* a_max, double* j_max) const {
+     549             : 
+     550          16 :   std::vector<int> dimensions;  // Evaluate in whatever dimensions we have.
+     551             : 
+     552          16 :   dimensions.push_back(3);  // 3 = heading
+     553             : 
+     554          16 :   Extremum v_min_traj, v_max_traj, a_min_traj, a_max_traj, j_min_traj, j_max_traj;
+     555             : 
+     556          16 :   bool success = computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::VELOCITY, dimensions, &v_min_traj, &v_max_traj);
+     557          16 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::ACCELERATION, dimensions, &a_min_traj, &a_max_traj);
+     558          16 :   success &= computeMinMaxMagnitude(eth_trajectory_generation::derivative_order::JERK, dimensions, &j_min_traj, &j_max_traj);
+     559             : 
+     560          16 :   *v_max = v_max_traj.value;
+     561          16 :   *a_max = a_max_traj.value;
+     562          16 :   *j_max = j_max_traj.value;
+     563             : 
+     564          32 :   return success;
+     565             : }
+     566             : 
+     567             : //}
+     568             : 
+     569             : /* scaleSegmentTimes() //{ */
+     570             : 
+     571           0 : bool Trajectory::scaleSegmentTimes(double scaling) {
+     572           0 :   if (scaling < 1.0e-6)
+     573           0 :     return false;
+     574             : 
+     575             :   // Scale the segment times of each segment.
+     576           0 :   double new_max_time    = 0.0;
+     577           0 :   double scaling_inverse = 1.0 / scaling;
+     578           0 :   for (size_t i = 0; i < segments_.size(); i++) {
+     579           0 :     double new_time = segments_[i].getTime() * scaling;
+     580           0 :     for (int d = 0; d < segments_[i].D(); d++) {
+     581           0 :       (segments_[i])[d].scalePolynomialInTime(scaling_inverse);
+     582             :     }
+     583           0 :     segments_[i].setTime(new_time);
+     584           0 :     new_max_time += new_time;
+     585             :   }
+     586           0 :   max_time_ = new_max_time;
+     587             : 
+     588           0 :   return true;
+     589             : }
+     590             : 
+     591             : //}
+     592             : 
+     593             : /* scaleSegmentTimesToMeetConstraints() //{ */
+     594             : 
+     595             : // This method SCALES the segment times evenly to ensure that the trajectory
+     596             : // is feasible given the provided v_max and a_max. Does not change the shape
+     597             : // of the trajectory, and only *increases* segment times.
+     598          16 : bool Trajectory::scaleSegmentTimesToMeetConstraints(const double v_max_horizontal, const double v_max_vertical, const double a_max_horizontal,
+     599             :                                                     const double a_max_vertical, const double j_max_horizontal, const double j_max_vertical,
+     600             :                                                     const double v_max_heading, const double a_max_heading, const double j_max_heading) {
+     601             : 
+     602             :   // In vast majority of cases, this will converge within 1 iteration.
+     603          16 :   constexpr size_t kMaxCounter = 20;
+     604          16 :   constexpr double kTolerance  = 1e-3;
+     605             : 
+     606          16 :   bool within_range = false;
+     607             : 
+     608          16 :   for (size_t i = 0; i < kMaxCounter; i++) {
+     609             : 
+     610         181 :     for (size_t seg = 0; seg < segments_.size(); seg++) {
+     611             : 
+     612             :       // From Liu, Sikang, et al. "Planning Dynamically Feasible Trajectories for
+     613             :       // Quadrotors Using Safe Flight Corridors in 3-D Complex Environments." IEEE
+     614             :       // Robotics and Automation Letters 2.3 (2017).
+     615             :       double v_max_horizontal_actual, a_max_horizontal_actual, j_max_horizontal_actual;
+     616         165 :       computeMaxDerivativesHorizontal(&v_max_horizontal_actual, &a_max_horizontal_actual, &j_max_horizontal_actual, seg);
+     617             : 
+     618             :       double v_max_vertical_actual, a_max_vertical_actual, j_max_vertical_actual;
+     619         165 :       computeMaxDerivativesVertical(&v_max_vertical_actual, &a_max_vertical_actual, &j_max_vertical_actual, seg);
+     620             : 
+     621             :       double v_max_heading_actual, a_max_heading_actual, j_max_heading_actual;
+     622         165 :       computeMaxDerivativesHeading(&v_max_heading_actual, &a_max_heading_actual, &j_max_heading_actual, seg);
+     623             : 
+     624             :       // Reevaluate constraint/bound violation
+     625         165 :       double velocity_violation_horizontal     = v_max_horizontal_actual / v_max_horizontal;
+     626         165 :       double velocity_violation_vertical       = v_max_vertical_actual / v_max_vertical;
+     627         165 :       double acceleration_violation_horizontal = a_max_horizontal_actual / a_max_horizontal;
+     628         165 :       double acceleration_violation_vertical   = a_max_vertical_actual / a_max_vertical;
+     629         165 :       double jerk_violation_horizontal         = j_max_horizontal_actual / j_max_horizontal;
+     630         165 :       double jerk_violation_vertical           = j_max_vertical_actual / j_max_vertical;
+     631             : 
+     632         165 :       double velocity_violation_heading     = v_max_heading_actual / v_max_heading;
+     633         165 :       double acceleration_violation_heading = a_max_heading_actual / a_max_heading;
+     634         165 :       double jerk_violation_heading         = j_max_heading_actual / j_max_heading;
+     635             : 
+     636         165 :       double velocity_violation     = std::max(std::max(velocity_violation_horizontal, velocity_violation_vertical), velocity_violation_heading);
+     637         165 :       double acceleration_violation = std::max(std::max(acceleration_violation_horizontal, acceleration_violation_vertical), acceleration_violation_heading);
+     638         165 :       double jerk_violation         = std::max(std::max(jerk_violation_horizontal, jerk_violation_vertical), jerk_violation_heading);
+     639             : 
+     640         165 :       within_range = velocity_violation <= 1.0 + kTolerance && acceleration_violation <= 1.0 + kTolerance && jerk_violation <= 1.0 + kTolerance;
+     641             : 
+     642         165 :       double violation_scaling = std::max(1.0, std::max(std::max(velocity_violation, sqrt(acceleration_violation)), cbrt(jerk_violation)));
+     643             : 
+     644             :       // First figure out how to stretch the trajectory in time.
+     645         165 :       double violation_scaling_inverse = 1.0 / violation_scaling;
+     646             : 
+     647             :       // Scale the segment times of each segment.
+     648         165 :       double new_max_time = 0.0;
+     649         165 :       double new_time     = segments_[seg].getTime() * violation_scaling;
+     650             : 
+     651         825 :       for (int d = 0; d < segments_[seg].D(); d++) {
+     652         660 :         (segments_[seg])[d].scalePolynomialInTime(violation_scaling_inverse);
+     653             :       }
+     654             : 
+     655         165 :       segments_[seg].setTime(new_time);
+     656         165 :       new_max_time += new_time;
+     657         165 :       max_time_ = new_max_time;
+     658             :     }
+     659             : 
+     660             :     double v_max_horizontal_actual, a_max_horizontal_actual, j_max_horizontal_actual;
+     661          16 :     computeMaxDerivativesHorizontal(&v_max_horizontal_actual, &a_max_horizontal_actual, &j_max_horizontal_actual);
+     662             : 
+     663             :     double v_max_vertical_actual, a_max_vertical_actual, j_max_vertical_actual;
+     664          16 :     computeMaxDerivativesVertical(&v_max_vertical_actual, &a_max_vertical_actual, &j_max_vertical_actual);
+     665             : 
+     666             :     double v_max_heading_actual, a_max_heading_actual, j_max_heading_actual;
+     667          16 :     computeMaxDerivativesHeading(&v_max_heading_actual, &a_max_heading_actual, &j_max_heading_actual);
+     668             : 
+     669             :     // Reevaluate constraint/bound violation
+     670          16 :     double velocity_violation_horizontal     = v_max_horizontal_actual / v_max_horizontal;
+     671          16 :     double velocity_violation_vertical       = v_max_vertical_actual / v_max_vertical;
+     672          16 :     double acceleration_violation_horizontal = a_max_horizontal_actual / a_max_horizontal;
+     673          16 :     double acceleration_violation_vertical   = a_max_vertical_actual / a_max_vertical;
+     674          16 :     double jerk_violation_horizontal         = j_max_horizontal_actual / j_max_horizontal;
+     675          16 :     double jerk_violation_vertical           = j_max_vertical_actual / j_max_vertical;
+     676             : 
+     677          16 :     double velocity_violation_heading     = v_max_heading_actual / v_max_heading;
+     678          16 :     double acceleration_violation_heading = a_max_heading_actual / a_max_heading;
+     679          16 :     double jerk_violation_heading         = j_max_heading_actual / j_max_heading;
+     680             : 
+     681          16 :     double velocity_violation     = std::max(std::max(velocity_violation_horizontal, velocity_violation_vertical), velocity_violation_heading);
+     682          16 :     double acceleration_violation = std::max(std::max(acceleration_violation_horizontal, acceleration_violation_vertical), acceleration_violation_heading);
+     683          16 :     double jerk_violation         = std::max(std::max(jerk_violation_horizontal, jerk_violation_vertical), jerk_violation_heading);
+     684             : 
+     685          16 :     within_range = velocity_violation <= 1.0 + kTolerance && acceleration_violation <= 1.0 + kTolerance && jerk_violation <= 1.0 + kTolerance;
+     686             : 
+     687          16 :     if (within_range) {
+     688          16 :       break;
+     689             :     }
+     690             :   }
+     691          16 :   return within_range;
+     692             : }
+     693             : 
+     694             : //}
+     695             : 
+     696             : }  // namespace eth_trajectory_generation
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.overview.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.overview.html new file mode 100644 index 0000000000..23dd5bb087 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.overview.html @@ -0,0 +1,194 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.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 + + +
    + Top

    + Overview +
    + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.png b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..370d8980784f98bcff0b50b21c9ffb926ca2527b GIT binary patch literal 2513 zcmV;?2`=`DP)g;1+ZTx&_^V-TzjuLI?p82(8Zj+9Pw$bUOVfBq0wBk8bFct?!4U z^~cxw&zx@zytn^{fdvAX!6iWVS$}}d0Q3jL01G&dM*-L*wHbg2ym9X`Vf>eY-up;L zCI$}X{KWIdcHP8d)(it|q3E<{K->0*-k=7CsB0V;z#Au-NsIy%f-hm1464YhXYj8u zibJpvzH3Ha0E(Zy+X6Ecm3!cI9H6uG!2@B8464dFAi5y+*q$1#rHDh8d>~6*<54Rw zQd9tS6xD6OkRZE@ZI>HQho<8dZ%6YJim%u9qHXuzk6&+VXx$os+fMNN8nEzx(fI+& z&-suuv&^k?b4R~+8k0HbNSLbSFHG@Dpfimkus1k&>tz*wIHy}Qu2?cOo=3>YPbogHy~?9n%v1sgxl>&f)Q z4Ijh?7}5bIe4<^P>LG(zQcqB0$;TR`cIIdXkVf=vr4zmxq?|w`+!6AfQkI4xyelgV zpHFwNEz7jTZ9czE?$y4WUrRT}ZHn>kb)Ltu8}UyGP5Yb4{WMMUNg<^uwv(c3YGB+L zpuTG_$K>=P0YmzD6~kWwj9U!6Jfw_dSk4iVrPit^&DTbXKelTa_=>`PzxQ=q~zGn*rE<_tKdxS5mR z0X4#mw(r^L(Py05zD!h~4mcBckSLx5u9>^)A96uzMc7k2m$r>UGW3Daw}xD(Oos3X zP|0s=(<4B$)aQ6tgbA_~bl)ilX8fY6LR7Q+u)+|}q?5M$Pq#J9>|cdS!P;6ZIF|s- zfHn2rE(xT8ccttUEUwGVmPfZdeVm=1<%f+vc(B#NP;@Nv2)S^pu$fIXWr~;Q!qty^ zjbhAV=}A*X@7tc@E^|ZK1xY3QXs&u~Cr;Z=ihsKUL<-+*HRUWB6iO+?%ywE0mw=bn zao1RC%Wfn%aGvm36Xs$b*_L_lhhfi`hN!WAS=c)P1&V9I#7Tq6M}0X(Kl#XKGZOHo zKw9tohvVZVOCWibYw=GwGkw%~qb60E-ZubSyPk5H)WC?9wg*Vn3O12?E{RSGY(kqH zftFQLt%Z~4;GzwWj*m#5Oxr49;Q6$ya3cF%Qrr4cae=L)!nh}Oxq=jw5loFMXi(9J zrc$W>nd=*PR5z&i)F~=^Zs_`n;r~_)$Esf`mXB7cv-hqk;bZcC*U+^#e~ilf5tX^+ zHZ3bF7(iSX>x_m+1Mr z93M3(53E#~e8kl?sHHgmv6{l{-hmPcZ0BTlGa(RFZo(l1zk?_m-WAG)8!ouYg(2W; z4=+3X)$W^5bTOvt`I_d+m3WabV(8dvfhawNTPvP&l;dkU z$5p@=LlMU@{7HQBCKm=kTgGq$P}%ShWHp>n!!1%=c_FhuOIPhfO9s7Rf^%tTpzMKnsOk!42Y#yT(X*47sUL53xAN#z{v}L z?0=v;_g(!KcAQUGDz-b(-#XdE)`Q;yHg!EM)$dId<5LPkvt7@MrLG2Gi`2p~iraxr zJTCQ917B9$e)6rTX_hrlKtmnQ&} zC_Vu|1U6B80>B>vn|NG)Gyr_Q+;u?!@F&Y%9C*VEZ}Mx{bjAXkC_aAuhrlKtm&dP5 z6d%7XQhfaS4}ncQF2Cp37p-7%=h*G4OR_}`mVE6>Q*piIh9zHEI>)=3#OsN}()qrY zRkUt;QZ#jE*WMXMD62(RTYGTn~?zj)%!70MWGRW$`_+i2(HG8)mYcJtfCzNy87~ zGiSV!H_oK8t_|05t!r|8>VvNNAG>1L^x#(OSW{_*(;}|xB&^VLfs30qvcNG0+Mjuf z$#h$!c+gZ)97i3GV+uryIz2L7AI>)?^dw4}6Qqm_O2(t~J1vzXaIA<3i0K zgf;T$^tRNVZf1AQJ_Cu}(C2aknOgLjiH2x;|6^mgOIz&&5N~3*7!FvVpf=&IM`!y!zHlWzHNb*JFmUYP#G& zlQT`R>vuDkE@zgyraiZ8iBaavPMI?o4mdLThy^Z7o-+lYj^a4#cpOt8QdH_unKOA| zn4Fmdg`D{|k7CYby4C;-IkT0=WvS)Nx30aF?x#lJ*|&<~O?N*mwf?`NtY~@@MU;U{ b>PPzz)xh-RK{OQ~00000NkvXXu0mjfu${+P literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.func-sort-c.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.func-sort-c.html new file mode 100644 index 0000000000..5b39bc1c78 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.func-sort-c.html @@ -0,0 +1,108 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - trajectory_sampling.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:296942.0 %
    Date:2024-04-09 22:03:33Functions:2728.6 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::sampleSegmentAtTime(eth_trajectory_generation::Segment const&, double, eth_mav_msgs::EigenTrajectoryPoint*)0
    bool eth_trajectory_generation::sampleFlatStateAtTime<eth_trajectory_generation::Trajectory>(eth_trajectory_generation::Trajectory const&, double, eth_mav_msgs::EigenTrajectoryPoint*)0
    bool eth_trajectory_generation::sampleFlatStateAtTime<eth_trajectory_generation::Segment>(eth_trajectory_generation::Segment const&, double, eth_mav_msgs::EigenTrajectoryPoint*)0
    eth_trajectory_generation::sampleTrajectoryAtTime(eth_trajectory_generation::Trajectory const&, double, eth_mav_msgs::EigenTrajectoryPoint*)0
    eth_trajectory_generation::sampleTrajectoryStartDuration(eth_trajectory_generation::Trajectory const&, double, double, double, std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> >*)0
    eth_trajectory_generation::sampleWholeTrajectory(eth_trajectory_generation::Trajectory const&, double, std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> >*)11
    eth_trajectory_generation::sampleTrajectoryInRange(eth_trajectory_generation::Trajectory const&, double, double, double, std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> >*)11
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.func.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.func.html new file mode 100644 index 0000000000..4e38616a01 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.func.html @@ -0,0 +1,108 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - trajectory_sampling.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:296942.0 %
    Date:2024-04-09 22:03:33Functions:2728.6 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::sampleSegmentAtTime(eth_trajectory_generation::Segment const&, double, eth_mav_msgs::EigenTrajectoryPoint*)0
    bool eth_trajectory_generation::sampleFlatStateAtTime<eth_trajectory_generation::Trajectory>(eth_trajectory_generation::Trajectory const&, double, eth_mav_msgs::EigenTrajectoryPoint*)0
    bool eth_trajectory_generation::sampleFlatStateAtTime<eth_trajectory_generation::Segment>(eth_trajectory_generation::Segment const&, double, eth_mav_msgs::EigenTrajectoryPoint*)0
    eth_trajectory_generation::sampleWholeTrajectory(eth_trajectory_generation::Trajectory const&, double, std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> >*)11
    eth_trajectory_generation::sampleTrajectoryAtTime(eth_trajectory_generation::Trajectory const&, double, eth_mav_msgs::EigenTrajectoryPoint*)0
    eth_trajectory_generation::sampleTrajectoryInRange(eth_trajectory_generation::Trajectory const&, double, double, double, std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> >*)11
    eth_trajectory_generation::sampleTrajectoryStartDuration(eth_trajectory_generation::Trajectory const&, double, double, double, std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> >*)0
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.frameset.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.frameset.html new file mode 100644 index 0000000000..44c3d833dd --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.html new file mode 100644 index 0000000000..197a80b489 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.html @@ -0,0 +1,271 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - trajectory_sampling.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:296942.0 %
    Date:2024-04-09 22:03:33Functions:2728.6 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #include <eth_trajectory_generation/trajectory_sampling.h>
+      22             : 
+      23             : namespace eth_trajectory_generation
+      24             : {
+      25             : 
+      26             : const double kNumNanosecondsPerSecond = 1.e9;
+      27             : 
+      28             : /* sampleTrajectoryAtTime() //{ */
+      29             : 
+      30           0 : bool sampleTrajectoryAtTime(const Trajectory& trajectory, double sample_time, eth_mav_msgs::EigenTrajectoryPoint* state) {
+      31           0 :   CHECK_NOTNULL(state);
+      32           0 :   if (sample_time < trajectory.getMinTime() || sample_time > trajectory.getMaxTime()) {
+      33           0 :     LOG(ERROR) << "Sample time should be within [" << trajectory.getMinTime() << " " << trajectory.getMaxTime() << "] but is " << sample_time;
+      34           0 :     return false;
+      35             :   }
+      36             : 
+      37           0 :   if (trajectory.D() < 3) {
+      38           0 :     LOG(ERROR) << "Dimension has to be at least 3, but is " << trajectory.D();
+      39           0 :     return false;
+      40             :   }
+      41             : 
+      42           0 :   return sampleFlatStateAtTime<Trajectory>(trajectory, sample_time, state);
+      43             : }
+      44             : 
+      45             : //}
+      46             : 
+      47             : /* sampleTrajectoryInRange() //{ */
+      48             : 
+      49          11 : bool sampleTrajectoryInRange(const Trajectory& trajectory, double min_time, double max_time, double sampling_interval,
+      50             :                              eth_mav_msgs::EigenTrajectoryPointVector* states) {
+      51          11 :   CHECK_NOTNULL(states);
+      52          11 :   if (min_time < trajectory.getMinTime() || max_time > trajectory.getMaxTime()) {
+      53           0 :     LOG(ERROR) << "Sample time should be within [" << trajectory.getMinTime() << " " << trajectory.getMaxTime() << "] but is [" << min_time << " " << max_time
+      54           0 :                << "]";
+      55           0 :     return false;
+      56             :   }
+      57             : 
+      58          11 :   if (trajectory.D() < 3) {
+      59           0 :     LOG(ERROR) << "Dimension has to be at least 3, but is " << trajectory.D();
+      60           0 :     return false;
+      61             :   }
+      62             : 
+      63          22 :   std::vector<Eigen::VectorXd> position, velocity, acceleration, jerk, snap, yaw, yaw_rate;
+      64             : 
+      65          11 :   trajectory.evaluateRange(min_time, max_time, sampling_interval, derivative_order::POSITION, &position);
+      66          11 :   trajectory.evaluateRange(min_time, max_time, sampling_interval, derivative_order::VELOCITY, &velocity);
+      67          11 :   trajectory.evaluateRange(min_time, max_time, sampling_interval, derivative_order::ACCELERATION, &acceleration);
+      68          11 :   trajectory.evaluateRange(min_time, max_time, sampling_interval, derivative_order::JERK, &jerk);
+      69          11 :   trajectory.evaluateRange(min_time, max_time, sampling_interval, derivative_order::SNAP, &snap);
+      70             : 
+      71          11 :   size_t n_samples = position.size();
+      72             : 
+      73          11 :   states->resize(n_samples);
+      74        1419 :   for (size_t i = 0; i < n_samples; ++i) {
+      75        1408 :     eth_mav_msgs::EigenTrajectoryPoint& state = (*states)[i];
+      76             : 
+      77             :     /* state.degrees_of_freedom = eth_mav_msgs::MavActuation::DOF4; */
+      78        1408 :     state.position_W         = position[i].head<3>();
+      79        1408 :     state.velocity_W         = velocity[i].head<3>();
+      80        1408 :     state.acceleration_W     = acceleration[i].head<3>();
+      81        1408 :     state.jerk_W             = jerk[i].head<3>();
+      82        1408 :     state.snap_W             = snap[i].head<3>();
+      83        1408 :     state.time_from_start_ns = static_cast<int64_t>((min_time + sampling_interval * i) * kNumNanosecondsPerSecond);
+      84        1408 :     if (trajectory.D() == 4) {
+      85        1408 :       state.setFromYaw(position[i](3));
+      86        1408 :       state.setFromYawRate(velocity[i](3));
+      87        1408 :       state.setFromYawAcc(acceleration[i](3));
+      88             :     }
+      89             :     /* else if (trajectory.D() == 6) { */
+      90             :     /*   // overactuated, write quaternion from interpolated rotation vector */
+      91             :     /*   Eigen::Vector3d rot_vec, rot_vec_vel, rot_vec_acc; */
+      92             :     /*   rot_vec = position[i].tail<3>(); */
+      93             :     /*   rot_vec_vel  = velocity[i].tail<3>(); */
+      94             :     /*   rot_vec_acc  = acceleration[i].tail<3>(); */
+      95             :     /*   Eigen::Matrix3d rot_matrix; */
+      96             :     /*   eth_mav_msgs::matrixFromRotationVector(rot_vec, &rot_matrix); */
+      97             :     /*   state.orientation_W_B = Eigen::Quaterniond(rot_matrix); */
+      98             :     /*   state.angular_velocity_W = eth_mav_msgs::omegaFromRotationVector(rot_vec, rot_vec_vel); */
+      99             :     /*   state.angular_acceleration_W = eth_mav_msgs::omegaDotFromRotationVector(rot_vec, rot_vec_vel, rot_vec_acc); */
+     100             :     /*   state.degrees_of_freedom = eth_mav_msgs::MavActuation::DOF6; */
+     101             :     /* } */
+     102             :   }
+     103          11 :   return true;
+     104             : }
+     105             : 
+     106             : //}
+     107             : 
+     108             : /* sampleTrajectoryStartDuration() //{ */
+     109             : 
+     110           0 : bool sampleTrajectoryStartDuration(const Trajectory& trajectory, double start_time, double duration, double sampling_interval,
+     111             :                                    eth_mav_msgs::EigenTrajectoryPointVector* states) {
+     112           0 :   return sampleTrajectoryInRange(trajectory, start_time, start_time + duration, sampling_interval, states);
+     113             : }
+     114             : 
+     115             : //}
+     116             : 
+     117             : /* sampleWholeTrajectory() //{ */
+     118             : 
+     119          11 : bool sampleWholeTrajectory(const Trajectory& trajectory, double sampling_interval, eth_mav_msgs::EigenTrajectoryPoint::Vector* states) {
+     120          11 :   const double min_time = trajectory.getMinTime();
+     121          11 :   const double max_time = trajectory.getMaxTime();
+     122             : 
+     123          11 :   return sampleTrajectoryInRange(trajectory, min_time, max_time, sampling_interval, states);
+     124             : }
+     125             : 
+     126             : //}
+     127             : 
+     128             : /* sampleSegmentAtTime() //{ */
+     129             : 
+     130           0 : bool sampleSegmentAtTime(const Segment& segment, double sample_time, eth_mav_msgs::EigenTrajectoryPoint* state) {
+     131           0 :   CHECK_NOTNULL(state);
+     132           0 :   if (sample_time < 0.0 || sample_time > segment.getTime()) {
+     133           0 :     LOG(ERROR) << "Sample time should be within [" << 0.0 << " " << segment.getTime() << "] but is " << sample_time;
+     134           0 :     return false;
+     135             :   }
+     136             : 
+     137           0 :   return sampleFlatStateAtTime<Segment>(segment, sample_time, state);
+     138             : }
+     139             : 
+     140             : //}
+     141             : 
+     142             : /* sampleFlatStateAtTime() //{ */
+     143             : 
+     144             : template <class T>
+     145           0 : bool sampleFlatStateAtTime(const T& type, double sample_time, eth_mav_msgs::EigenTrajectoryPoint* state) {
+     146           0 :   if (type.D() < 3) {
+     147           0 :     LOG(ERROR) << "Dimension has to be 3, 4, or 6 but is " << type.D();
+     148           0 :     return false;
+     149             :   }
+     150             : 
+     151           0 :   Eigen::VectorXd position     = type.evaluate(sample_time, derivative_order::POSITION);
+     152           0 :   Eigen::VectorXd velocity     = type.evaluate(sample_time, derivative_order::VELOCITY);
+     153           0 :   Eigen::VectorXd acceleration = type.evaluate(sample_time, derivative_order::ACCELERATION);
+     154             : 
+     155             :   /* state->degrees_of_freedom = eth_mav_msgs::MavActuation::DOF4; */
+     156           0 :   state->position_W     = position.head(3);
+     157           0 :   state->velocity_W     = velocity.head(3);
+     158           0 :   state->acceleration_W = acceleration.head(3);
+     159           0 :   state->jerk_W         = type.evaluate(sample_time, derivative_order::JERK).head(3);
+     160           0 :   state->snap_W         = type.evaluate(sample_time, derivative_order::SNAP).head(3);
+     161             : 
+     162           0 :   if (type.D() == 4) {
+     163           0 :     state->setFromYaw(position(3));
+     164           0 :     state->setFromYawRate(velocity(3));
+     165           0 :     state->setFromYawAcc(acceleration(3));
+     166             :   }
+     167             :   /* else if (type.D() == 6) { */
+     168             :   /*   // overactuated, write quaternion from interpolated rotation vector */
+     169             :   /*   Eigen::Vector3d rot_vec, rot_vec_vel, rot_vec_acc; */
+     170             :   /*   rot_vec  = position.tail(3); */
+     171             :   /*   rot_vec_vel = velocity.tail(3); */
+     172             :   /*   rot_vec_acc = acceleration.tail(3); */
+     173             :   /*   Eigen::Matrix3d rot_matrix; */
+     174             :   /*   eth_mav_msgs::matrixFromRotationVector(rot_vec, &rot_matrix); */
+     175             :   /*   state->orientation_W_B = Eigen::Quaterniond(rot_matrix); */
+     176             :   /*   state->angular_velocity_W = eth_mav_msgs::omegaFromRotationVector(rot_vec, rot_vec_vel); */
+     177             :   /*   state->angular_acceleration_W = eth_mav_msgs::omegaDotFromRotationVector(rot_vec, rot_vec_vel, rot_vec_acc); */
+     178             :   /*   state->degrees_of_freedom = eth_mav_msgs::MavActuation::DOF6; */
+     179             :   /* } */
+     180             : 
+     181           0 :   state->time_from_start_ns = static_cast<int64_t>(sample_time * kNumNanosecondsPerSecond);
+     182           0 :   return true;
+     183             : }
+     184             : 
+     185             : //}
+     186             : 
+     187             : }  // namespace eth_trajectory_generation
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.overview.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.overview.html new file mode 100644 index 0000000000..b362418f82 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.overview.html @@ -0,0 +1,67 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.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_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.png b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/trajectory_sampling.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..8c01d1873eb304f02b3b89269e8d5936e4c2c5a7 GIT binary patch literal 941 zcmV;e15*5nP)0000OP)t-s|NsB0 zs;a7|?_FK${{R500003000620&P%hU00009a7bBm000ie000ie0hKEb8vp*nVgr7h_@Y=7>^j@uaOdvTzLWL5OvTe%A=05&U z`U{EI;a@2hh+qa+0pZ@}z*3=pB^lrV$MvxTTpG1hs01(Isu@iDNVL8YA4v*S(%*@F zV@)shu@T7tS2Tk6Bzil1*<|v7BBoPd5HDe5vLXdKB%N}YWO7k-kBVoEqRI^6O*7*~ zVE4{d$(>>3vPO9ly&$)bMX1E=WLplrIiry%>#%!CJ|FWb#a2gy-^5X1j)6EJK4Xbm zXf)ZHI6$SbV9FrMLQ8%Ufab(*FnxxIz-c*>dyKII?PM^%vj&D?!&jTM9KRh*9qlAB z%%NXm((;1oKe3+INTvxtjs8{R`>o$ZG(GPA><;2ZqQ2lc@4w?gBD%t(tFdi zue>useDev-e#ggGa=6y}K5)+yUam6Mku~`cPT^1V$JvmAV^t>Uj^}3pS4K@=2A&yL z{Wt{r?(X~UY-{Yj=1n)l-u;7hf9LyHFYP2_fyEwa_9_Qb&JNT~SAZT$<{JV{K)B4n zIzH<_5M9lr(Vu}md8*jim?Vu&XjuAiKo-ga85%j@UV`ay%Ww@nTC6@QVKZa%>154?PAGKj z^RnhUlIA9B=8|TZ!{I`cnVp$R7l5-2PdP=~Q&sD4E!B? z()ipeLwrya?H7S$ORWY@O0${Lq+gLHX##L=vc>n-kIO*1Vc0+;Fx)8jI%-xe0B7A3 z`RGV>0z5ucI&5lz9ViE6H*^+=n~RT82RG#AB1^PG=QSTyApf0Qi(%;5e3b$7=DX81gH P00000NkvXXu0mjfL@=@_ literal 0 HcmV?d00001 diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.func-sort-c.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.func-sort-c.html new file mode 100644 index 0000000000..5df99e526f --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.func-sort-c.html @@ -0,0 +1,148 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - vertex.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:13527050.0 %
    Date:2024-04-09 22:03:33Functions:61735.3 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::createRandomVertices(int, unsigned long, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, unsigned long)0
    eth_trajectory_generation::createSquareVertices(int, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double, int)0
    eth_trajectory_generation::createRandomVertices1D(int, unsigned long, double, double, unsigned long)0
    eth_trajectory_generation::computeTimeVelocityRamp(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, double, double)0
    eth_trajectory_generation::estimateSegmentTimesVelocityRamp(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&, double, double, double)0
    eth_trajectory_generation::Vertex::removeConstraint(int)0
    eth_trajectory_generation::operator<<(std::basic_ostream<char, std::char_traits<char> >&, eth_trajectory_generation::Vertex const&)0
    eth_trajectory_generation::operator<<(std::basic_ostream<char, std::char_traits<char> >&, std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&)0
    eth_trajectory_generation::Vertex::isEqualTol(eth_trajectory_generation::Vertex const&, double) const0
    eth_trajectory_generation::Vertex::hasConstraint(int) const0
    eth_trajectory_generation::Vertex::getSubdimension(std::vector<unsigned long, std::allocator<unsigned long> > const&, int, eth_trajectory_generation::Vertex*) const0
    eth_trajectory_generation::estimateSegmentTimes(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&, double, double, double, double, double, double, double, double)22
    eth_trajectory_generation::estimateSegmentTimesBaca(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&, double, double, double, double, double, double, double, double)22
    eth_trajectory_generation::estimateSegmentTimesEuclidean(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&, double, double, double, double, double, double, double, double)22
    eth_trajectory_generation::Vertex::makeStartOrEnd(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, int)32
    eth_trajectory_generation::Vertex::addConstraint(int, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)534
    eth_trajectory_generation::Vertex::getConstraint(int, Eigen::Matrix<double, -1, 1, 0, -1, 1>*) const2728
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.func.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.func.html new file mode 100644 index 0000000000..6f9d047d67 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.func.html @@ -0,0 +1,148 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - vertex.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:13527050.0 %
    Date:2024-04-09 22:03:33Functions:61735.3 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    eth_trajectory_generation::createRandomVertices(int, unsigned long, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, unsigned long)0
    eth_trajectory_generation::createSquareVertices(int, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double, int)0
    eth_trajectory_generation::estimateSegmentTimes(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&, double, double, double, double, double, double, double, double)22
    eth_trajectory_generation::createRandomVertices1D(int, unsigned long, double, double, unsigned long)0
    eth_trajectory_generation::computeTimeVelocityRamp(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, double, double)0
    eth_trajectory_generation::estimateSegmentTimesBaca(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&, double, double, double, double, double, double, double, double)22
    eth_trajectory_generation::estimateSegmentTimesEuclidean(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&, double, double, double, double, double, double, double, double)22
    eth_trajectory_generation::estimateSegmentTimesVelocityRamp(std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&, double, double, double)0
    eth_trajectory_generation::Vertex::addConstraint(int, Eigen::Matrix<double, -1, 1, 0, -1, 1> const&)534
    eth_trajectory_generation::Vertex::makeStartOrEnd(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, int)32
    eth_trajectory_generation::Vertex::removeConstraint(int)0
    eth_trajectory_generation::operator<<(std::basic_ostream<char, std::char_traits<char> >&, eth_trajectory_generation::Vertex const&)0
    eth_trajectory_generation::operator<<(std::basic_ostream<char, std::char_traits<char> >&, std::vector<eth_trajectory_generation::Vertex, std::allocator<eth_trajectory_generation::Vertex> > const&)0
    eth_trajectory_generation::Vertex::isEqualTol(eth_trajectory_generation::Vertex const&, double) const0
    eth_trajectory_generation::Vertex::getConstraint(int, Eigen::Matrix<double, -1, 1, 0, -1, 1>*) const2728
    eth_trajectory_generation::Vertex::hasConstraint(int) const0
    eth_trajectory_generation::Vertex::getSubdimension(std::vector<unsigned long, std::allocator<unsigned long> > const&, int, eth_trajectory_generation::Vertex*) const0
    +
    +
    + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.frameset.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.frameset.html new file mode 100644 index 0000000000..16b13edb26 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.frameset.html @@ -0,0 +1,19 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp + + + + + + + + <center>Frames not supported by your browser!<br></center> + + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.html new file mode 100644 index 0000000000..81e81f0e7c --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.html @@ -0,0 +1,674 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src/eth_trajectory_generation - vertex.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:13527050.0 %
    Date:2024-04-09 22:03:33Functions:61735.3 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

    +
              Line data    Source code
    +
    +       1             : /*
+       2             :  * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
+       3             :  * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
+       4             :  * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
+       5             :  * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
+       6             :  * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
+       7             :  *
+       8             :  * Licensed under the Apache License, Version 2.0 (the "License");
+       9             :  * you may not use this file except in compliance with the License.
+      10             :  * You may obtain a copy of the License at
+      11             :  *
+      12             :  * http://www.apache.org/licenses/LICENSE-2.0
+      13             :  *
+      14             :  * Unless required by applicable law or agreed to in writing, software
+      15             :  * distributed under the License is distributed on an "AS IS" BASIS,
+      16             :  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+      17             :  * See the License for the specific language governing permissions and
+      18             :  * limitations under the License.
+      19             :  */
+      20             : 
+      21             : #include <random>
+      22             : #include <iostream>
+      23             : 
+      24             : #include <eth_trajectory_generation/vertex.h>
+      25             : 
+      26             : #include <mrs_lib/geometry/cyclic.h>
+      27             : 
+      28             : namespace eth_trajectory_generation
+      29             : {
+      30             : 
+      31             : /* createRandomVertices() //{ */
+      32             : 
+      33           0 : Vertex::Vector createRandomVertices(int maximum_derivative, size_t n_segments, const Eigen::VectorXd& pos_min, const Eigen::VectorXd& pos_max, size_t seed) {
+      34           0 :   CHECK_GE(static_cast<int>(n_segments), 1);
+      35           0 :   CHECK_EQ(pos_min.size(), pos_max.size());
+      36           0 :   CHECK_GE((pos_max - pos_min).norm(), 0.2);
+      37           0 :   CHECK_GT(maximum_derivative, 0);
+      38             : 
+      39           0 :   Vertex::Vector                                      vertices;
+      40           0 :   std::mt19937                                        generator(seed);
+      41           0 :   std::vector<std::uniform_real_distribution<double>> distribution;
+      42             : 
+      43           0 :   const size_t dimension = pos_min.size();
+      44             : 
+      45           0 :   distribution.resize(dimension);
+      46             : 
+      47           0 :   for (size_t i = 0; i < dimension; ++i) {
+      48           0 :     distribution[i] = std::uniform_real_distribution<double>(pos_min[i], pos_max[i]);
+      49             :   }
+      50             : 
+      51           0 :   const double min_distance = 0.2;
+      52           0 :   const size_t n_vertices   = n_segments + 1;
+      53             : 
+      54           0 :   Eigen::VectorXd last_pos(dimension);
+      55           0 :   for (size_t i = 0; i < dimension; ++i) {
+      56           0 :     last_pos[i] = distribution[i](generator);
+      57             :   }
+      58             : 
+      59           0 :   vertices.reserve(n_segments + 1);
+      60           0 :   vertices.push_back(Vertex(dimension));
+      61             : 
+      62           0 :   vertices.front().makeStartOrEnd(last_pos, maximum_derivative);
+      63             : 
+      64           0 :   for (size_t i = 1; i < n_vertices; ++i) {
+      65           0 :     Eigen::VectorXd pos(dimension);
+      66             : 
+      67             :     while (true) {
+      68           0 :       for (size_t d = 0; d < dimension; ++d) {
+      69           0 :         pos[d] = distribution[d](generator);
+      70             :       }
+      71           0 :       if ((pos - last_pos).norm() > min_distance) {
+      72           0 :         break;
+      73             :       }
+      74           0 :     }
+      75             : 
+      76           0 :     Vertex v(dimension);
+      77           0 :     v.addConstraint(derivative_order::POSITION, pos);
+      78           0 :     vertices.push_back(v);
+      79           0 :     last_pos = pos;
+      80             :   }
+      81           0 :   vertices.back().makeStartOrEnd(last_pos, maximum_derivative);
+      82             : 
+      83           0 :   return vertices;
+      84             : }
+      85             : 
+      86             : //}
+      87             : 
+      88             : /* createSquareVertices() //{ */
+      89             : 
+      90           0 : Vertex::Vector createSquareVertices(int maximum_derivative, const Eigen::Vector3d& center, double side_length, int rounds) {
+      91           0 :   Vertex::Vector vertices;
+      92           0 :   const size_t   dimension = center.size();
+      93             : 
+      94           0 :   Eigen::Vector3d pos1(center[0] - side_length / 2.0, center[1] - side_length / 2.0, center[2]);
+      95           0 :   Vertex          v1(dimension);
+      96           0 :   v1.addConstraint(derivative_order::POSITION, pos1);
+      97           0 :   Eigen::Vector3d pos2(center[0] - side_length / 2.0, center[1] + side_length / 2.0, center[2]);
+      98           0 :   Vertex          v2(dimension);
+      99           0 :   v2.addConstraint(derivative_order::POSITION, pos2);
+     100           0 :   Eigen::Vector3d pos3(center[0] + side_length / 2.0, center[1] + side_length / 2.0, center[2]);
+     101           0 :   Vertex          v3(dimension);
+     102           0 :   v3.addConstraint(derivative_order::POSITION, pos3);
+     103           0 :   Eigen::Vector3d pos4(center[0] + side_length / 2.0, center[1] - side_length / 2.0, center[2]);
+     104           0 :   Vertex          v4(dimension);
+     105           0 :   v4.addConstraint(derivative_order::POSITION, pos4);
+     106             : 
+     107           0 :   vertices.reserve(4 * rounds);
+     108           0 :   vertices.push_back(v1);
+     109           0 :   vertices.front().makeStartOrEnd(pos1, maximum_derivative);
+     110             : 
+     111           0 :   for (int i = 0; i < rounds; ++i) {
+     112           0 :     vertices.push_back(v2);
+     113           0 :     vertices.push_back(v3);
+     114           0 :     vertices.push_back(v4);
+     115           0 :     vertices.push_back(v1);
+     116             :   }
+     117           0 :   vertices.back().makeStartOrEnd(pos1, maximum_derivative);
+     118             : 
+     119           0 :   return vertices;
+     120             : }
+     121             : 
+     122             : //}
+     123             : 
+     124             : /* createRandomVertices1D() //{ */
+     125             : 
+     126           0 : Vertex::Vector createRandomVertices1D(int maximum_derivative, size_t n_segments, double pos_min, double pos_max, size_t seed) {
+     127           0 :   return createRandomVertices(maximum_derivative, n_segments, Eigen::VectorXd::Constant(1, pos_min), Eigen::VectorXd::Constant(1, pos_max), seed);
+     128             : }
+     129             : 
+     130             : //}
+     131             : 
+     132             : /* addConstraint() //{ */
+     133             : 
+     134         534 : void Vertex::addConstraint(int derivative_order, const Eigen::VectorXd& constraint) {
+     135         534 :   CHECK_EQ(constraint.rows(), static_cast<long>(D_));
+     136         534 :   constraints_[derivative_order] = constraint;
+     137         534 : }
+     138             : 
+     139             : //}
+     140             : 
+     141             : /* removeConstraint() //{ */
+     142             : 
+     143           0 : bool Vertex::removeConstraint(int type) {
+     144           0 :   Constraints::const_iterator it = constraints_.find(type);
+     145           0 :   if (it != constraints_.end()) {
+     146           0 :     constraints_.erase(it);
+     147           0 :     return true;
+     148             :   } else {
+     149             :     // Constraint not found.
+     150           0 :     return false;
+     151             :   }
+     152             : }
+     153             : 
+     154             : //}
+     155             : 
+     156             : /* makeStartOrEnd() //{ */
+     157             : 
+     158          32 : void Vertex::makeStartOrEnd(const Eigen::VectorXd& constraint, int up_to_derivative) {
+     159          32 :   addConstraint(derivative_order::POSITION, constraint);
+     160          96 :   for (int i = 1; i <= up_to_derivative; ++i) {
+     161          64 :     constraints_[i] = ConstraintValue::Zero(static_cast<int>(D_));
+     162             :   }
+     163          32 : }
+     164             : 
+     165             : //}
+     166             : 
+     167             : /* getConstraint() //{ */
+     168             : 
+     169        2728 : bool Vertex::getConstraint(int derivative_order, Eigen::VectorXd* value) const {
+     170        2728 :   CHECK_NOTNULL(value);
+     171        2728 :   typename Constraints::const_iterator it = constraints_.find(derivative_order);
+     172        2728 :   if (it != constraints_.end()) {
+     173        1484 :     *value = it->second;
+     174        1484 :     return true;
+     175             :   } else
+     176        1244 :     return false;
+     177             : }
+     178             : 
+     179             : //}
+     180             : 
+     181             : /* hasConstraint() //{ */
+     182             : 
+     183           0 : bool Vertex::hasConstraint(int derivative_order) const {
+     184           0 :   typename Constraints::const_iterator it = constraints_.find(derivative_order);
+     185           0 :   return it != constraints_.end();
+     186             : }
+     187             : 
+     188             : //}
+     189             : 
+     190             : /* isEqualTol() //{ */
+     191             : 
+     192           0 : bool Vertex::isEqualTol(const Vertex& rhs, double tol) const {
+     193           0 :   if (constraints_.size() != rhs.constraints_.size())
+     194           0 :     return false;
+     195             :   // loop through lhs constraint map
+     196           0 :   for (typename Constraints::const_iterator it = cBegin(); it != cEnd(); ++it) {
+     197             :     // look for matching key
+     198           0 :     typename Constraints::const_iterator rhs_it = rhs.constraints_.find(it->first);
+     199           0 :     if (rhs_it == rhs.constraints_.end())
+     200           0 :       return false;
+     201             :     // check value
+     202           0 :     if (!((it->second - rhs_it->second).isZero(tol)))
+     203           0 :       return false;
+     204             :   }
+     205           0 :   return true;
+     206             : }
+     207             : 
+     208             : //}
+     209             : 
+     210             : /* getSubdimension() //{ */
+     211             : 
+     212           0 : bool Vertex::getSubdimension(const std::vector<size_t>& subdimensions, int max_derivative_order, Vertex* subvertex) const {
+     213           0 :   CHECK_NOTNULL(subvertex);
+     214           0 :   *subvertex = Vertex(subdimensions.size());
+     215             : 
+     216             :   // Check if all subdimensions exist.
+     217           0 :   for (size_t subdimension : subdimensions)
+     218           0 :     if (subdimension >= D_)
+     219           0 :       return false;
+     220             : 
+     221             :   // Copy constraints up to maximum derivative order.
+     222           0 :   for (Constraints::const_iterator it = constraints_.begin(); it != constraints_.end(); ++it) {
+     223           0 :     int derivative_order = it->first;
+     224           0 :     if (derivative_order > max_derivative_order)
+     225           0 :       continue;
+     226           0 :     const ConstraintValue& original_constraint = it->second;
+     227           0 :     ConstraintValue        subsconstraint(subvertex->D());
+     228           0 :     for (size_t i = 0; i < subdimensions.size(); i++) {
+     229           0 :       subsconstraint[i] = original_constraint[subdimensions[i]];
+     230             :     }
+     231           0 :     subvertex->addConstraint(derivative_order, subsconstraint);
+     232             :   }
+     233           0 :   return true;
+     234             : }
+     235             : 
+     236             : //}
+     237             : 
+     238             : /* operator<<(std::ostream& stream, const Vertex& v) //{ */
+     239             : 
+     240           0 : std::ostream& operator<<(std::ostream& stream, const Vertex& v) {
+     241           0 :   stream << "constraints: " << std::endl;
+     242           0 :   Eigen::IOFormat format(4, 0, ", ", "\n", "[", "]");
+     243           0 :   for (typename Vertex::Constraints::const_iterator it = v.cBegin(); it != v.cEnd(); ++it) {
+     244           0 :     stream << "  type: " << positionDerivativeToString(it->first);
+     245           0 :     stream << "  value: " << it->second.transpose().format(format) << std::endl;
+     246             :   }
+     247           0 :   return stream;
+     248             : }
+     249             : 
+     250             : //}
+     251             : 
+     252             : /* operator<<(std::ostream& stream, const std::vector<Vertex>& vertices) //{ */
+     253             : 
+     254           0 : std::ostream& operator<<(std::ostream& stream, const std::vector<Vertex>& vertices) {
+     255           0 :   for (const Vertex& v : vertices) {
+     256           0 :     stream << v << std::endl;
+     257             :   }
+     258           0 :   return stream;
+     259             : }
+     260             : 
+     261             : //}
+     262             : 
+     263             : /* estimateSegmentTimes() //{ */
+     264             : 
+     265          22 : std::vector<double> estimateSegmentTimes(const Vertex::Vector& vertices, const double v_max_horizontal, const double v_max_vertical,
+     266             :                                          const double a_max_horizontal, const double a_max_vertical, const double j_max_horizontal, const double j_max_vertical,
+     267             :                                          const double heading_speed_max, const double heading_acc_max) {
+     268             : 
+     269             :   return estimateSegmentTimesEuclidean(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical,
+     270          22 :                                        heading_speed_max, heading_acc_max);
+     271             : }
+     272             : 
+     273             : //}
+     274             : 
+     275             : /* estimateSegmentTimesVelocityRamp() //{ */
+     276             : 
+     277           0 : std::vector<double> estimateSegmentTimesVelocityRamp(const Vertex::Vector& vertices, double v_max, double a_max, double time_factor) {
+     278           0 :   CHECK_GE(vertices.size(), 2);
+     279           0 :   std::vector<double> segment_times;
+     280             : 
+     281           0 :   segment_times.reserve(vertices.size() - 1);
+     282             : 
+     283           0 :   constexpr double kMinSegmentTime = 0.1;
+     284             : 
+     285           0 :   for (size_t i = 0; i < vertices.size() - 1; ++i) {
+     286           0 :     Eigen::VectorXd start, end;
+     287           0 :     vertices[i].getConstraint(derivative_order::POSITION, &start);
+     288           0 :     vertices[i + 1].getConstraint(derivative_order::POSITION, &end);
+     289           0 :     double t = computeTimeVelocityRamp(start, end, v_max, a_max);
+     290           0 :     t        = std::max(kMinSegmentTime, t);
+     291           0 :     segment_times.push_back(t);
+     292             :   }
+     293             : 
+     294           0 :   return segment_times;
+     295             : }
+     296             : 
+     297             : //}
+     298             : 
+     299             : /* estimateSegmentTimesBaca() //{ */
+     300             : 
+     301          22 : std::vector<double> estimateSegmentTimesBaca(const Vertex::Vector& vertices, const double v_max_horizontal, const double v_max_vertical,
+     302             :                                              const double a_max_horizontal, const double a_max_vertical, const double j_max_horizontal,
+     303             :                                              const double j_max_vertical, const double heading_speed_max, const double heading_acc_max) {
+     304             : 
+     305          22 :   CHECK_GE(vertices.size(), 2);
+     306          22 :   std::vector<double> segment_times;
+     307          22 :   segment_times.reserve(vertices.size() - 1);
+     308             : 
+     309             :   // for each vertex in the path
+     310         209 :   for (size_t i = 0; i < vertices.size() - 1; ++i) {
+     311             : 
+     312         374 :     Eigen::VectorXd start4d, end4d;
+     313             : 
+     314         187 :     vertices[i].getConstraint(derivative_order::POSITION, &start4d);
+     315         187 :     vertices[i + 1].getConstraint(derivative_order::POSITION, &end4d);
+     316             : 
+     317         187 :     Eigen::Vector3d start     = start4d.head(3);
+     318         187 :     Eigen::Vector3d end       = end4d.head(3);
+     319         187 :     double          start_hdg = start4d(3);
+     320         187 :     double          end_hdg   = end4d(3);
+     321             : 
+     322         187 :     double acceleration_time_1 = 0;
+     323         187 :     double acceleration_time_2 = 0;
+     324             : 
+     325         187 :     double jerk_time_1 = 0;
+     326         187 :     double jerk_time_2 = 0;
+     327             : 
+     328         187 :     double acc_1_coeff = 0;
+     329         187 :     double acc_2_coeff = 0;
+     330             : 
+     331         187 :     double distance = (end - start).norm();
+     332             : 
+     333         187 :     double inclinator = atan2(end(2) - start(2), sqrt(pow(end(0) - start(0), 2) + pow(end(1) - start(1), 2)));
+     334             : 
+     335             :     double v_max, a_max, j_max;
+     336             : 
+     337         187 :     if (inclinator > atan2(v_max_vertical, v_max_horizontal) || inclinator < -atan2(v_max_vertical, v_max_horizontal)) {
+     338           0 :       v_max = fabs(v_max_vertical / sin(inclinator));
+     339             :     } else {
+     340         187 :       v_max = fabs(v_max_horizontal / cos(inclinator));
+     341             :     }
+     342             : 
+     343         187 :     if (inclinator > atan2(a_max_vertical, a_max_horizontal) || inclinator < -atan2(a_max_vertical, a_max_horizontal)) {
+     344           0 :       a_max = fabs(a_max_vertical / sin(inclinator));
+     345             :     } else {
+     346         187 :       a_max = fabs(a_max_horizontal / cos(inclinator));
+     347             :     }
+     348             : 
+     349         187 :     if (inclinator > atan2(j_max_vertical, j_max_horizontal) || inclinator < -atan2(j_max_vertical, j_max_horizontal)) {
+     350           0 :       j_max = fabs(j_max_vertical / sin(inclinator));
+     351             :     } else {
+     352         187 :       j_max = fabs(j_max_horizontal / cos(inclinator));
+     353             :     }
+     354             : 
+     355         187 :     if (i >= 1) {
+     356             : 
+     357         165 :       Eigen::VectorXd pre4d;
+     358             : 
+     359         165 :       vertices[i - 1].getConstraint(derivative_order::POSITION, &pre4d);
+     360             : 
+     361         165 :       Eigen::Vector3d pre = pre4d.head(3);
+     362             : 
+     363         165 :       Eigen::Vector3d vec1 = start - pre;
+     364         165 :       Eigen::Vector3d vec2 = end - start;
+     365             : 
+     366         165 :       vec1.normalize();
+     367         165 :       vec2.normalize();
+     368             : 
+     369         165 :       double scalar = vec1.dot(vec2) < 0 ? 0.0 : vec1.dot(vec2);
+     370             : 
+     371         165 :       acc_1_coeff = (1 - scalar);
+     372             : 
+     373         165 :       acceleration_time_1 = acc_1_coeff * ((v_max / a_max) + (a_max / j_max));
+     374             : 
+     375         165 :       jerk_time_1 = acc_1_coeff * (2 * (a_max / j_max));
+     376             :     }
+     377             : 
+     378             :     // the first vertex
+     379         187 :     if (i == 0) {
+     380          22 :       acc_1_coeff         = 1.0;
+     381          22 :       acceleration_time_1 = (v_max / a_max) + (a_max / j_max);
+     382          22 :       jerk_time_1         = (2 * (a_max / j_max));
+     383             :     }
+     384             : 
+     385             :     // last vertex
+     386         187 :     if (i == vertices.size() - 2) {
+     387          22 :       acc_2_coeff         = 1.0;
+     388          22 :       acceleration_time_2 = (v_max / a_max) + (a_max / j_max);
+     389          22 :       jerk_time_2         = (2 * (a_max / j_max));
+     390             :     }
+     391             : 
+     392             :     // a vertex
+     393         187 :     if (i < vertices.size() - 2) {
+     394             : 
+     395         165 :       Eigen::VectorXd post4d;
+     396             : 
+     397         165 :       vertices[i + 2].getConstraint(derivative_order::POSITION, &post4d);
+     398             : 
+     399         165 :       Eigen::Vector3d post = post4d.head(3);
+     400             : 
+     401         165 :       Eigen::Vector3d vec1 = end - start;
+     402         165 :       Eigen::Vector3d vec2 = post - end;
+     403             : 
+     404         165 :       vec1.normalize();
+     405         165 :       vec2.normalize();
+     406             : 
+     407         165 :       double scalar = vec1.dot(vec2) < 0 ? 0.0 : vec1.dot(vec2);
+     408             : 
+     409         165 :       acc_2_coeff = (1 - scalar);
+     410             : 
+     411         165 :       acceleration_time_2 = acc_2_coeff * ((v_max / a_max) + (a_max / j_max));
+     412             : 
+     413         165 :       jerk_time_2 = acc_2_coeff * (2 * (a_max / j_max));
+     414             :     }
+     415             : 
+     416         187 :     if (acceleration_time_1 > sqrt(2 * distance / a_max)) {
+     417          18 :       acceleration_time_1 = sqrt(2 * distance / a_max);
+     418             :     }
+     419             : 
+     420         187 :     if (jerk_time_1 > sqrt(2 * v_max / j_max)) {
+     421           0 :       jerk_time_1 = sqrt(2 * v_max / j_max);
+     422             :     }
+     423             : 
+     424         187 :     if (acceleration_time_2 > sqrt(2 * distance / a_max)) {
+     425          20 :       acceleration_time_2 = sqrt(2 * distance / a_max);
+     426             :     }
+     427             : 
+     428         187 :     if (jerk_time_2 > sqrt(2 * v_max / j_max)) {
+     429           0 :       jerk_time_2 = sqrt(2 * v_max / j_max);
+     430             :     }
+     431             : 
+     432             :     double max_velocity_time;
+     433             : 
+     434         187 :     const double distance_due_acceleration = a_max * pow(acceleration_time_1, 2) + a_max * pow(acceleration_time_2, 2);
+     435             : 
+     436         187 :     if (distance > distance_due_acceleration) {
+     437          80 :       max_velocity_time = ((distance - distance_due_acceleration) / v_max);
+     438             :     } else {
+     439         107 :       max_velocity_time = (distance_due_acceleration / v_max);
+     440             :     }
+     441             : 
+     442         187 :     max_velocity_time = ((distance) / v_max);
+     443             : 
+     444             :     /* double t = max_velocity_time + acceleration_time_1 + acceleration_time_2 + jerk_time_1 + jerk_time_2; */
+     445         187 :     double t = max_velocity_time + acceleration_time_1 + acceleration_time_2;
+     446             : 
+     447             :     /* printf("segment %d, [%.2f %.2f %.2f] - > [%.2f %.2f %.2f] = %.2f\n", i, start(0), start(1), start(2), end(0), end(1), end(2), distance); */
+     448             :     /* printf("segment %d time %.2f, distance %.2f, %.2f, %.2f, %.2f, vmax: %.2f, amax: %.2f, jmax: %.2f\n", i, t, distance, max_velocity_time, */
+     449             :     /*        acceleration_time_1, acceleration_time_2, v_max, a_max, j_max); */
+     450             : 
+     451         187 :     if (t < 0.01) {
+     452           0 :       t = 0.01;
+     453             :     }
+     454             : 
+     455             :     // | ------------- check the heading rotation time ------------ |
+     456             : 
+     457         187 :     double angular_distance = fabs(mrs_lib::geometry::radians::dist(start_hdg, end_hdg));
+     458             : 
+     459         187 :     double hdg_velocity_time     = 0;
+     460         187 :     double hdg_acceleration_time = 0;
+     461             : 
+     462         187 :     if (heading_speed_max < std::numeric_limits<float>::max() && heading_acc_max < std::numeric_limits<float>::max()) {
+     463             : 
+     464         187 :       if (((angular_distance - (2 * (heading_speed_max * heading_speed_max) / heading_acc_max)) / heading_speed_max) < 0) {
+     465         184 :         hdg_velocity_time = ((angular_distance) / heading_speed_max);
+     466             :       } else {
+     467           3 :         hdg_velocity_time = ((angular_distance - (2 * (heading_speed_max * heading_speed_max) / heading_acc_max)) / heading_speed_max);
+     468             :       }
+     469             : 
+     470         187 :       if (angular_distance > M_PI / 4) {
+     471          43 :         hdg_acceleration_time = 2 * (heading_speed_max / heading_acc_max);
+     472             :       }
+     473             :     }
+     474             : 
+     475             :     // what will take longer? to fix the lateral or the heading
+     476         187 :     double heading_fix_time = 1.5 * (hdg_velocity_time + hdg_acceleration_time);
+     477             : 
+     478         187 :     if (heading_fix_time > t) {
+     479           0 :       t = heading_fix_time;
+     480             :     }
+     481             : 
+     482         187 :     segment_times.push_back(t);
+     483             :   }
+     484          22 :   return segment_times;
+     485             : }
+     486             : 
+     487             : //}
+     488             : 
+     489             : /* estimateSegmentTimesEuclidean() //{ */
+     490             : 
+     491          22 : std::vector<double> estimateSegmentTimesEuclidean(const Vertex::Vector& vertices, const double v_max_horizontal, const double v_max_vertical,
+     492             :                                                   const double a_max_horizontal, const double a_max_vertical, const double j_max_horizontal,
+     493             :                                                   const double j_max_vertical, const double heading_speed_max, const double heading_acc_max) {
+     494             : 
+     495          22 :   double v_max = std::min(v_max_horizontal, v_max_vertical);
+     496             : 
+     497          22 :   CHECK_GE(vertices.size(), 2);
+     498          22 :   std::vector<double> segment_times;
+     499          22 :   segment_times.reserve(vertices.size() - 1);
+     500             : 
+     501             :   // for each vertex in the path
+     502         209 :   for (size_t i = 0; i < vertices.size() - 1; ++i) {
+     503             : 
+     504         374 :     Eigen::VectorXd start4d, end4d;
+     505             : 
+     506         187 :     vertices[i].getConstraint(derivative_order::POSITION, &start4d);
+     507         187 :     vertices[i + 1].getConstraint(derivative_order::POSITION, &end4d);
+     508             : 
+     509         187 :     Eigen::Vector3d start = start4d.head(3);
+     510         187 :     Eigen::Vector3d end   = end4d.head(3);
+     511             : 
+     512         187 :     double inclinator = atan2(end(2) - start(2), sqrt(pow(end(0) - start(0), 2) + pow(end(1) - start(1), 2)));
+     513             : 
+     514             :     double v_max;
+     515             : 
+     516         187 :     if (inclinator > atan2(v_max_vertical, v_max_horizontal) || inclinator < -atan2(v_max_vertical, v_max_horizontal)) {
+     517           0 :       v_max = fabs(v_max_vertical / sin(inclinator));
+     518             :     } else {
+     519         187 :       v_max = fabs(v_max_horizontal / cos(inclinator));
+     520             :     }
+     521             : 
+     522         187 :     double start_hdg = start4d(3);
+     523         187 :     double end_hdg   = end4d(3);
+     524             : 
+     525         187 :     double distance = (end - start).norm();
+     526             : 
+     527         187 :     double t = distance / v_max;
+     528             : 
+     529         187 :     if (t < 0.01) {
+     530           0 :       t = 0.01;
+     531             :     }
+     532             : 
+     533             :     // | ------------- check the heading rotation time ------------ |
+     534             : 
+     535             : 
+     536         187 :     double angular_distance = fabs(mrs_lib::geometry::radians::dist(start_hdg, end_hdg));
+     537             : 
+     538         187 :     double hdg_velocity_time     = 0;
+     539         187 :     double hdg_acceleration_time = 0;
+     540             : 
+     541         187 :     if (heading_speed_max < std::numeric_limits<float>::max() && heading_acc_max < std::numeric_limits<float>::max()) {
+     542             : 
+     543         187 :       if (((angular_distance - ((heading_speed_max * heading_speed_max) / heading_acc_max)) / heading_speed_max) < 0) {
+     544         146 :         hdg_velocity_time = ((angular_distance) / heading_speed_max);
+     545             :       } else {
+     546          41 :         hdg_velocity_time = ((angular_distance - ((heading_speed_max * heading_speed_max) / heading_acc_max)) / heading_speed_max);
+     547             :       }
+     548             : 
+     549         187 :       if (angular_distance > M_PI / 4) {
+     550          43 :         hdg_acceleration_time = 2 * (heading_speed_max / heading_acc_max);
+     551             :       }
+     552             :     }
+     553             : 
+     554             :     // what will take longer? to fix the lateral or the heading
+     555         187 :     double heading_fix_time = 1.5 * (hdg_velocity_time + hdg_acceleration_time);
+     556             : 
+     557         187 :     if (heading_fix_time > t) {
+     558          22 :       t = heading_fix_time;
+     559             :     }
+     560             : 
+     561         187 :     segment_times.push_back(t);
+     562             :   }
+     563             : 
+     564          22 :   return segment_times;
+     565             : }
+     566             : 
+     567             : //}
+     568             : 
+     569             : /* computeTimeVelocityRamp() //{ */
+     570             : 
+     571           0 : double computeTimeVelocityRamp(const Eigen::VectorXd& start, const Eigen::VectorXd& goal, double v_max, double a_max) {
+     572             : 
+     573           0 :   const double distance = (start - goal).norm();
+     574             :   // Time to accelerate or decelerate to or from maximum velocity:
+     575           0 :   const double acc_time = v_max / a_max;
+     576             :   // Distance covered during complete acceleration or decelerate:
+     577           0 :   const double acc_distance = 0.5 * v_max * acc_time;
+     578             :   // Compute total segment time:
+     579           0 :   if (distance < 2.0 * acc_distance) {
+     580             :     // Case 1: Distance too small to accelerate to maximum velocity.
+     581           0 :     return 2.0 * std::sqrt(distance / a_max);
+     582             :   } else {
+     583             :     // Case 2: Distance long enough to accelerate to maximum velocity.
+     584           0 :     return 2.0 * acc_time + (distance - 2.0 * acc_distance) / v_max;
+     585             :   }
+     586             : }
+     587             : 
+     588             : //}
+     589             : 
+     590             : }  // namespace eth_trajectory_generation
+
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.overview.html b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.overview.html new file mode 100644 index 0000000000..11851bbde6 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.overview.html @@ -0,0 +1,168 @@ + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.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 + + +
    + Top

    + Overview +
    + + diff --git a/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.png b/mrs_uav_trajectory_generation/src/eth_trajectory_generation/vertex.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..ea9e12c3ef970ad5f7610f5aae6ac9b48316848d GIT binary patch literal 2241 zcmV;y2tN0TP)}P>#oA+Q(!6 zbNn5TFC%<+|E7rr1Yia(1N8SEJMcIG{%K?YD{vf-0$`Wa;{-_Hi~m*#gMUV#{z!B* zrhwCU+(F)0(p@|jV`Kn(FnaBcplx^Y7=s3;gmf?rz>i+CF>?w~Ncf0m8iR^ZJrjO{ z6{qMSJZVN=02J?>9mq@*ap3wg7NQ;+wtfTS6+Dg__AWQfKqVQ4dDS;IKh`Y{n_T%j z>+ssjv2L$oq=1Om)&Q*~;4O?iRRC0ufRVYEaEpK(l8xeE)sv{xp5QU)e?>jAfE^f9 z&O|^xM(gZncMew&cI7b^wl2njV0cMPd}#xg^7Un<@sFKlAnHs6PYEzK0Rhy z6Ktaf6G;bq*WNTq;;O#VQZ(H%JfSBSx2)Kz&6HRjSZRf)0Jg08lJ3N$}t4( zv;dVXc7&Ac^2UJi#}g#{5-iiwC9X5CWM$`Lk5b`zjKYw%`rp9pwRQ+|wc)!`IZD`GJW>WFrDDkH#W zMT(yY&A~`Vn5HJIYRUtqelc@K+5vY5i8o3kM};qPgf9W>KpxPvff9@@=g@nP-qs)B zJ=r?5l8Yo_`*kK=lUA;z>oX}!K&6QLL7CKL@itdDZhr7*)VW))$+9l4$sKFqwgN#Y zaSNt_8tIl=7gGzc?dn=X9;K~ei#+D;!}J6^#*HQ_ykM?SsSz76w$l_Oy_Ia08*wG+ zo_(Prk@~1Pb77P!wq|r@;BQt^i4)z1D!izh3x#u~!d=6D#Qw0HzrJwNQx1d%1l(Q4 zmJ9NzoeCYokOL3c^?no`?^hNi(lx<`c|5!bBy=<+9?2p^>?26sjCwxy&^~jDJOd2Y z$cq6P#=MCH$kSaF^5}4K?N0)z?tL(5m~Q)@k3AEfzX`{1&c!11}-A5^R%sN+*SOH0$Q5~)h!Cp?LF-pz$bf8SkAiFw7U1i zyI^Z~57I12;b219!4UAzoA8>fJ+F3T+v|A=`)g+S`lul&%)(M32RqT%)Q1`ecQTDT zEBVMSI3DLhKJ#h2;ma(Z%*i4)#?LI#RJGU;r39#*TwE`X@_r zLJTjWxKJ4`qqsmPjpAe1`Q@a-Zsd9;0Yr|PfTTprAlBEz2Yd_wcDfZ|jH70bRsibw zC>76VNym1iW`$)M=a2&|Epuqb_g$O8u3Zm)aby))qz;T>wPhms-EHA7g7VJjbJ{ zWHcwGJ{aBqNlRP;I16dT8PcsDdk0_->6tj}JcCS$O8bRzSzLwDQ|bvhioqXq;Qbtr zgjHjx!~l5pbcJlVy}b3z>*x`c0}iDggV4@UB1T>}H>#&`jEkx#IIX0o-=`i`V&v3g zqBOHaTE*bASQTW!u3Z((c^-=x_70DlV4naQ!PUO2!gp2Rr)wE}P@tOB1n#GJ40Ijs zBC>@3wM(m4d@hj5n$`X5uO5F`k7;k{idwDPAT9Z^(>aU&Mk=N<3P(Mr%LjXAIL61Y z2~7TrxBB4n18JqKW26!#slpB3lTt2yHt?t=?ccu=+KBNgxy19hb0TYTX}U>6ukqpE zPUAW8FO7lM%{&soF}xpVEdCyV;JtY>GZd93=?ycY!d)iSO2pkG%f)dFZWi7$UGc3q z@!c~MYSG!T2iY*BRafb(z0|*E)sxn7pHHc*nHPM;`e%Qn;AM%sh`v-IsYx61g^+R> z8PfDJU{nPk31h*7!>AIb!zqZ=KF4JlPIz<;9Zo$cwUWnM-i|Xu%z^hWJwlI)tabdEK9H|%!jwP zInMPwf(f(qiA?BVYpn^t{p%{}-`l?yyVrUD`l90ga#yQTKYLn}>#w%2mSi8-G~v?& zR&1>_;awf^SW38r$3M9HIN?3Z^4+Fzq?2t5bLj~E + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/srcHitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:747109568.2 %
    Date:2024-04-09 22:03:33Functions:2222100.0 %
    Legend: Rating: + low: < 75 % + medium: >= 75 % + high: >= 90 % +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

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

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

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

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

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

    Filename Sort by nameLine Coverage ( show details ) Sort by line coverageFunctions Sort by function coverage
    mrs_trajectory_generation.cpp +
    68.2%68.2%
    +     		68.2 %747 / 1095100.0 %22 / 22
    +
    +
    + + + + +
    Generated by: LCOV version 1.14
    +
    + + + diff --git a/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func-sort-c.html b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func-sort-c.html new file mode 100644 index 0000000000..7041f8e5e6 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func-sort-c.html @@ -0,0 +1,168 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src - mrs_trajectory_generation.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:747109568.2 %
    Date:2024-04-09 22:03:33Functions: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()82
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackDrs(mrs_uav_trajectory_generation::drsConfig&, unsigned int)82
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::onInit()82
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::interpolatePoint(Waypoint_t const&, Waypoint_t const&, double const&)898
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::distFromSegment(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)4480
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::overtime()9806
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)145603
    +
    +
    + + + +
    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..952c1a9902 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.func.html @@ -0,0 +1,168 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp - functions + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src - mrs_trajectory_generation.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:747109568.2 %
    Date:2024-04-09 22:03:33Functions:2222100.0 %
    Legend: Lines: + hit + not hit +
    +
    + +
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +

    Function Name Sort by function nameHit count Sort by hit count
    (anonymous namespace)::ProxyExec0::ProxyExec0()82
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackDrs(mrs_uav_trajectory_generation::drsConfig&, unsigned int)82
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackPath(boost::shared_ptr<mrs_msgs::Path_<std::allocator<void> > const>)1
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::trajectorySrv(mrs_msgs::TrajectoryReference_<std::allocator<void> > const&)4
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::transformPath(mrs_msgs::Path_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)6
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectory(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, double const&, bool const&)16
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::preprocessPath(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&)11
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackPathSrv(mrs_msgs::PathSrvRequest_<std::allocator<void> >&, mrs_msgs::PathSrvResponse_<std::allocator<void> >&)3
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::distFromSegment(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&)4480
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackUavState(boost::shared_ptr<mrs_msgs::UavState_<std::allocator<void> > const>)145603
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::interpolatePoint(Waypoint_t const&, Waypoint_t const&, double const&)898
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::callbackGetPathSrv(mrs_msgs::GetPathSrvRequest_<std::allocator<void> >&, mrs_msgs::GetPathSrvResponse_<std::allocator<void> >&)2
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectoryAsync(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, double const&, bool const&)16
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::findTrajectoryFallback(std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&, double const&, bool const&)6
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::getTrajectoryReference(std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> > const&, std::optional<mrs_msgs::TrackerCommand_<std::allocator<void> > > const&, double const&)6
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::prepareInitialCondition(ros::Time)11
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::validateTrajectorySpatial(std::vector<eth_mav_msgs::EigenTrajectoryPoint, Eigen::aligned_allocator<eth_mav_msgs::EigenTrajectoryPoint> > const&, std::vector<Waypoint_t, std::allocator<Waypoint_t> > const&)17
    mrs_uav_trajectory_generation::MrsTrajectoryGeneration::onInit()82
    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()9806
    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..f2c22313c4 --- /dev/null +++ b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.html @@ -0,0 +1,2458 @@ + + + + + + + LCOV - MRS UAV System - Test coverage report - mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp + + + + + + + + + + + + + + +
    LCOV - code coverage report
    + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Current view:top level - mrs_uav_trajectory_generation/src - mrs_trajectory_generation.cpp (source / functions)HitTotalCoverage
    Test:MRS UAV System - Test coverage reportLines:747109568.2 %
    Date:2024-04-09 22:03:33Functions:2222100.0 %
    Legend: Lines: + hit + not hit +
    +
    + + + + + + + + +

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

    + Overview +
    + + diff --git a/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.png b/mrs_uav_trajectory_generation/src/mrs_trajectory_generation.cpp.gcov.png new file mode 100644 index 0000000000000000000000000000000000000000..cf3cf28fbf6511f73c13293d7772fdf0bf5020f9 GIT binary patch literal 7274 zcmV-w9F^mVP)9`~RB zZll@{K8N(tLJ5EkPz>0_YUB|&CE$G^>0`Lw!(#;_0n}rxaF>%d@M!3aW~^lVoghx@3C21|73ZsQbk;@!VhtWdXz@xw<1E|JW<5ba(@OY4e zh;f7o4H(CviWsMz$7vwmeX;(*+1A}|z)?Kjr{>)DfEp&%q)!>bv19lM-9|ljY)`ox zRsuR!?k&J6O2GtJ<59+F?R6VaFX$w_md-@7E%m-1*1v-CcztBlhXou?Iao8S6~e5b z9NwLMNk89cZ^YPyNw*OS(xYuKJu~Z;4v~eO5+|%-%Jmv zCw(8=m4e3sh=0_pQ#_I;9|9CMhBw~Z6H&K&{fMeMXmCUx8PdvL-{oHS+Pwo~Ep6FV zdshRG(-em$+7oAi!)noIQsP**03&$z7rCnefEj2Fko;8x5F{u87Gfv?9|U87!m-O|cMG%YP(eQI_?(`yoa{70f#jCN-my$=Ub=R1bG z&g_!V+bAGIhVJ=V`P)(@feq4beEFhhw$Dxco~%fEePUX1w$CP zC(Y1a)Bad6QZBSm}uVz*~5fTJ*iT1nz}wAU)N{ z6oAJ!XH?QU9?5b*q-5a-tusU+<98kFjP{>QaYb`+uE0nL+1_!WW73jN7+VUG0Gk*s zq&?5rnPYfS$6Gq1BF~y8CS~|8%Uqr>mD8v zOH=-JAI3+qAn6h)CM_UG8XumA-xI#+u3ifx#)+ajzI+f4lYPx)7+%HWRORe;4I5=*U@fcUwcs)H2l1by;%St}Ta1PyWATjd{|zRWMpe4?Yk}JPJ~ZBX^8Pve@?Xh*@Ig zNXB&$Weyl4pr93$G!~JFaS0eR^^!?90Ru(>j`B!5L%XCO!YBThb1`*OGXr{7G#Fj0 z0-8D_V{(77m@rZepC0)Z?vREToZ_GDLnz5{K8CR}KL3O@Lt>*m#?IJ!4U?)d3(@Qo z=e3PAZJY&S0Wsv70QICnYV(ijk$cV3E<1mFN+@zb!&sw{tuQI_Sb6{_@F=#7vA9h)(0?#F^$|V!!T03<1j{)2EgN_qm+j87;ujTT(p3m$6+HA zxHOP{KaZA?G01Tl$73=a-40R=LL6V%1vd`96%1)<*jYecJYBsWa=c$38L;TNvFC;{ z7Y`N*2iQ5*W1Zo=Be?g9$u$?f<^f?%p9O2mqjG3wG!I=3S z-9eXU89`z_zT4N=civw6|F7@u=Re=Rei{P&s8IU+B1ZU7zWHesp?+o!18g&DIsH)kKA9j z@|dDraA~l8)HTx|X-v}{5%zzC^jJ!D3YVLvkX-0MS&Z=(6dWGK zMa}P_c9AXQ`W%XuxMvMDWoSzvmIwqf`*Z(fS zwe(X;Kuc$E`;pV`A*}wl1p(?Y<~wv83w7i0^M}D1{&Jq?t8hzw+tQ~JaN9B%|HVz` zk2)VWVSKR+Yz5#3fbmm?(M&pa;|F2sN*gi0fVCQ+9^>X1+=6)bPd___tprqKWCuI@ z;)Mb8kr_l9q22uVTMx#BW!t{9wuQe87|R~#|Kx_?+}~A6e^meopk{xzuStWKO)Q;P zQ;y$hvQ+_9d(H8Eab~YrWtc-1iy=j;VbO5#iU%zAe>T$0F)9sfF5Wa3;eyR;@5_Y^ zHuvk5<_wyuA3fGyB5gT3N#Pin_h`y6j>pKIwQ_(W03Jf^SjPdL8(Y#-PWNU;90f-8 zzEIsxO8P=ukK>yRVdi`P9ZmQ{K|nr@EUbin-K);R;jz7(N0z}SIKxY8CZ40XGc#d1 z9<`)NGUo)_>+bumG=U|l&H)~ufX8hgWk5r(UEh3%D_z73+`ch9PWl-Wm*R=A1PBY< zi#p@kDt5|LfHk~kolOGAnaua3Y4@0s88X??Ed0@NF@MECns*eo;3GKq+F2aEdmU}= z6uS~HrXChkk>pPmNZj3OqkAz4FgFl-p6sZ^im>Hl=0NlSyfTTO! zoe5YFd6~xyF#KcI>@gN~GhsA$rWRk@0d>BD!8#5%xMkMo;(zU_qN|G%JD;p4)*MeC zc(>wAhaFRAJogj)M<$d4P8|izB_4Md1xB5GPACeP1)7)yn~-q&IXm`|_q3=uay7$Q zcEiP^ZYLcBq747^@yHD^I8a3=z3I!NNezw8m;v9L5>v^FCuG`y$4NJDUo|>zzBX8F z*_g4$2fed=`2soi)p)bpoNa#3^a^KAnSTO+Oxwai&z1qgQPcN#al{p~>`sU(pix1e z^VO#7)SgFn!BT%(uh6ET!WDzplP-otGt1|5J?E-g6)Xd#^s+&iSD*2HV zhcJFXDO%rLWc>yI#slA;KFHD3Ac^z_Xb+N>d(B5@{BN`WZU18lr=;xNLvl-$kZ$-C zEz(P@2VfzsQ3}0=bEKD&h10H8SIrKQW8zW<)QZQtx`q=sq^S%b)Bd&9b(fFJDa}<4 z4Y(!YlyNkZ#<>6O_L1B<$u|qY_<=OrgwrKNU_doSq1!$sI9QZ*K7?^0Dvz$GQ>Z7n z(a@}&S;9<=NMo!vkGiTcK4HKbfNG2Z<&6F3d^iI}7!dB3=ALVa0R5-zVq?G^J$r@e za}#Xr1|)T*mGq_K(6%b*yoy>{Bk?Ly&TwmKcb(CA6*B3}t2jW~+-o2Who2p!tNX$> z=`ALQs7QZ(={)hMk4I3ovII&O#J;o5zH4MeCUeIQyf&IZCC4k+V8EAYBBD zwaj7wG(Zke9q{4$lGZ}-bVKg-J}L?x#bL_;|B1j*oCFJa>Ol%|LBbJQK`RWjArGy7 z(8qgwWr|ugT1e{woV~xMwRk9xTGA*o9FHy8lGl*T&5R}Ly^-`CE@q}IyF2AaA9PWZ zM)#F!Nlm82G?X$Ju1eoy?NnTimF{OY$SW!n{C@rxrPX9y?mxRZBX1=CHTXIe|bmG&EG27f8<1xz*9e~jz%3YvM04#l_ zF48YPDHW-^{a#$lJA>x_CqQn}0?Fz&VMY#upwnF0R{8F`(j_QXTT06!;} z>!bn3AUUPC0sPq-P0`ag73P0;ng=)0Tm+nVL&R)qZw^7rKnh5=Bq1yEmN`2-%vZ&OI!LU_W|@pQRfpR}10;;Ei zgh|{sb0i6%l{7-A24vN8K(8f0@f|j_d}v9N5J?44i!mLYyTj+|4oTPa3a+f`2-`g= znG2ge-N_5z zL#I-#Lw*L}#pA`J#N)s)cf5Skz98b*S71EJ>^1V-p4NFSoOyyvBKYZ{vk4Rt<3JfgiMkhE( z0Fy}Tn>J!61>!DDcd*+)4+1lrt%(xrNOnU%oT2!SLpJNp<2f&UuVVi+jK~iyH2nv!qxjnC25s zQ_p?VZph6_X+4eF9orwgRk`4~y2?d#^cV}_rP2 zSD;k)6|@+yiw&S-#o!#l$hQ8{lUJJUpH8zqBkPm;;KRhrey&s(UTvUV5cTMBb4enrdzZ zoef{A0B75#qUdz&cp)Bbuj5|1Rj`%YyShT`%i$sP!O#(<)t)HnmM$Wl7+ow*cY2bX zgF(SNxW(#hX06w1@jR(qgx6b5Av;~Qrf2NUQhQY@kdC~e0b~JH6)NCPZsW-x%bi?x zQ--eti+gq2I%(9G7U1YxY@)^Wzs+;7xu(fAg|ovDwVUABl1`&kD)}DIGpDR>apW;* z8A)!L^NJO4>%7IIzKFL$j&ylt0)UZ69qHJanb~T>TrwSWi&fLBfG}Yj7{>!jU_ElI zAnU+5J5^j3;`YoXpbq0cXQ7HSUs&n&GS~3+m7TY=H)OVeTzy;_qqgkr=;E_y6se6t z?&er4l_J;KU={(rppNfs>7QL@9$`7a0v@#X+J0v2z3dWNvrmgbkA9z%fe(n+6)fvaCjl|)H zD)FPosdHgy%pPfo_txYHJ5A}WOsnFCG&{vs3U6|e!-tQQ?WBFhQ44A5+|1%=Cem%y zeS~tU&g#{8Ed9dBDCV~}s!;YE9dvhM%vC5_UlYbOD9poiUA-o!nu*1H4Mzf(11=Hp z+-KmJnKB1oavJ0S2`0n~QpgaTg3+NDS*eMTwxkFmdX9)J0zDrCaa0Hxmo_aE|h z!Gt|Ce4V1t4JY2-09Ws3%G=Aly_mR~&9YlC&FO^nIFFUP0BM^P6j!Sh*k`Vd!?RdD z*hS8E4I#vNWdAk@T#He84}lAjz{vx;(-i>Q*Uo?2QWPT-Is)mEN9%{Pk+{#WJ!9>J zvlNxgjF`dRDez^f#8eO>#}&|BVMEO~lwkI(Jd9Bhr;`R)n@Q9#^tQsYhpu zbD=U`W5lTGjJjoyjdB|w*AQSx1I$jm;`1ohG+Q!!Y{&IG_KyoSop5R3c)Uj)YBI2% z8GxnsM6YhF@g}8l_$jD#kqD%nTpUIo@WeEUWfAE9Iac$$V+@>T%jXZeq# z9^Ui%u<|WLFPDz=r{_N88o-!A7%k9%n$k-;P!Av94wy-Bfqq##BL%@QpatU}n+n6; zpSrvFpg}Y17_QTL-NtxQz_%Ud`k)QuSPj>y_(y4Yfiy$k8?r-g zX?`^1ZN281byKAVUO@T5qOvoev7ey_*fbmONIKrQStA8q$my6d-wX|O4bZ8XVl6(c zNbShrRUW7mE9Bejk-lWNH3D+}Wgr4&Nq}Si7}1)}0-ohRM&dOdwVlB!^D*GX0dnQvvWU`I+C#4^R3?|Y z*QhhuekEy)&FlL*7nJl|W`U7om{J-1W2rxR*EyKw4znr$jN#(;ii@$dMjvI?~ zy!>h^mKIhlc>RF}~Wb#{X~g0F=G{%OmrTrXVPL z{ijCe%6_O~RJT*JAFBBe!sB;k{==31aAiNBpUwLCm$M&MA@0az(1Aq z(e`I3U?q&5?y)!Th+kPGp$X!-vPc5XD~kkUys}8V9s0hqNTR@hFN;KcM05?2Dfd(n zJQtN?Et49qERs5)8si6Lk?>VvXvH^Jc>HU3DO3b;=2mh3*S>Q|1YrG;RWuF01jWJ^qGbRHr$Q>`Yq++*oKm(7` zQ?N+^tr+Kq(%N|ZN-7>tJ%WzI;fGT`X652l8~__Co&i=)Q&j35G<zbKgi^WxT=8pyU z3o){>X}#hq;htw?XidcngZqyMFviFdZY-cmy?-?3KYw0U8$5TS1^@s607*qoM6N<$ Ef^EFk1^@s6 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